81 lines
2.1 KiB
Python
81 lines
2.1 KiB
Python
"""
|
|
Source: https://bitbucket.org/thesheep/micropython-d1motor
|
|
|
|
import d1motor
|
|
from machine import I2C, Pin
|
|
i2c = I2C(Pin(5), Pin(4), freq=100000)
|
|
m0 = d1motor.Motor(0, i2c)
|
|
m1 = d1motor.Motor(1, i2c)
|
|
m0.speed(5000)
|
|
|
|
"""
|
|
import ustruct
|
|
import utime
|
|
import math
|
|
|
|
sign = lambda x: math.copysign(1, x)
|
|
|
|
|
|
_STATE_BRAKE = const(0)
|
|
_STATE_RIGHT = const(1) # clockwise
|
|
_STATE_LEFT = const(2) # counter-clockwise
|
|
_STATE_STOP = const(3)
|
|
_STATE_SLEEP = const(4)
|
|
|
|
|
|
class Motor:
|
|
def __init__(self, index, i2c, address=0x30, standby_pin=None):
|
|
if index not in (0, 1):
|
|
raise ValueError("Index must be 0 or 1")
|
|
self.index = index
|
|
self.i2c = i2c
|
|
self.address = address
|
|
self.standby_pin = standby_pin
|
|
self._speed = 0
|
|
self._state = 0
|
|
if standby_pin is not None:
|
|
standby_pin.init(standby_pin.OUT, 0)
|
|
self.frequency(1000)
|
|
|
|
def frequency(self, frequency=None):
|
|
if frequency is None:
|
|
return self._pwm_frequency
|
|
self._pwm_frequency = frequency
|
|
self.i2c.writeto_mem(self.address, 0x00 | self.index,
|
|
ustruct.pack(">BH", 0x00, frequency))
|
|
|
|
def update(self):
|
|
if self.standby_pin is not None:
|
|
self.standby_pin.value(not self._state == _STATE_SLEEP)
|
|
self.i2c.writeto_mem(self.address, 0x10 | self.index,
|
|
ustruct.pack(">BH", self._state, self._speed))
|
|
|
|
def speed(self, speed=None):
|
|
if speed is None:
|
|
return self._speed
|
|
if abs(speed) < 30 and abs(speed) > 10: # kick-start for small speeds
|
|
self.speed(int(100 * sign(speed)))
|
|
utime.sleep_ms(5)
|
|
|
|
if speed > 0:
|
|
self._speed = min(10000, max(1, speed))
|
|
self._state = _STATE_RIGHT
|
|
elif speed < 0:
|
|
self._speed = min(10000, max(1, -speed))
|
|
self._state = _STATE_LEFT
|
|
else:
|
|
self._speed = 0
|
|
self._state = _STATE_STOP
|
|
self.update()
|
|
|
|
def sleep(self):
|
|
self._speed = 0
|
|
self._state = _STATE_SLEEP
|
|
self.update()
|
|
|
|
def brake(self):
|
|
self._speed = 0
|
|
self._state = _STATE_BRAKE
|
|
self.update()
|
|
|