RoboRally/micropython_firmware/d1motor.py

81 lines
2.1 KiB
Python
Raw Normal View History

"""
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()