import struct
from numbers import Real
from typing import Optional
# Major and minor version of required firmware
_REQUIRED_FIRMWARE_VERSION = (0, 4)
class FirmwareVersionMismatch(Exception):
pass
class WhoAmIMismatch(Exception):
pass
class Controller:
I2C_ADDR = 0x57
FIRMWARE_VERSION = 0x08
WHO_AM_I = 0x0F
PWM_FREQUENCY = 0x10
MAX_MOTOR_PERCENTAGE = 0x11
MOTOR_SHUTDOWN_TIMEOUT = 0x28
MOTOR_SPEED = 0x30
AUTOMATIC_MOTOR_SPEED = 0x31
ENCODER_TICKS = 0x32
STATUS = 0x36
RESET = 0xE0
RESET_TO_BOOTLOADER = 0xE1
DEVICE_ID = 0xF0
def __init__(self, i2c_bus=8):
import smbus
self.i2c_bus = i2c_bus
self.i2c = smbus.SMBus(self.i2c_bus)
self.check_who_am_i()
self.check_firmware_version()
def _read(self, command, n, unpack_spec) -> tuple:
return struct.unpack(
unpack_spec,
bytes(self.i2c.read_i2c_block_data(self.I2C_ADDR, command, n)),
)
def _write(self, command: int, data: list[int]):
self.i2c.write_i2c_block_data(self.I2C_ADDR, command, data)
def who_am_i(self) -> int:
"""Check that the motors controller board is present. This
should return the same value as Controller.I2C_ADDR."""
return self._read(self.WHO_AM_I, 1, "B")[0]
def check_who_am_i(self):
"""Check that the device answers to WHO_AM_I is correct."""
w = self.who_am_i()
if w != self.I2C_ADDR:
error = (
f"WHO_AM_I returns {w:#04x} "
f"instead of the expected {self.I2C_ADDR:#04x}"
)
raise WhoAmIMismatch(error)
def set_max_percentage(self, percent: float):
"""Set the maximum percentage of power which will be used
(between 1 and 100) for further speed instructions. This has
no effect on an running command."""
if percent <= 0 or percent > 100:
raise ValueError("percentage must be between 0 and 100")
self._write(self.MAX_MOTOR_PERCENTAGE, [round(percent)])
def get_max_percentage(self) -> int:
"""Get the maximum percentage of power which will be used
(between 1 and 100)."""
return self._read(self.MAX_MOTOR_PERCENTAGE, 1, "B")[0]
def set_motor_speed(self, left: Optional[float], right: Optional[float]):
"""Set the motor speed between -100 and 100. None means not to
change the motor value. Using None for both motors will put
the controller board in standby mode and motors will stop."""
def convert(v: Optional[float], arg: str):
if v is None:
return -128
if not isinstance(v, Real) or v < -100 or v > 100:
raise ValueError(
f"{arg} motor speed "
"must be a number between -100 and 100, or None"
)
return round(v)
self._write(
self.MOTOR_SPEED,
list(struct.pack("bb", convert(left, "left"), convert(right, "right"))),
)
def get_motor_speed(self) -> Optional[tuple[int, int]]:
"""Get the left and right motor speed as a tuple, or None if in standby."""
(left, right) = self._read(self.MOTOR_SPEED, 2, "bb")
return (left, right) if left != 0x80 and right != 0x80 else None
def set_automatic_motor_speed(self, left: int, right: int):
"""Set the motor speed in ticks by 10th of seconds."""
def check(v: int, arg: str):
if v < -32767 or v > 32767:
raise ValueError(
f"{arg} automatic motor speed must be a number "
+ "between -32767 and 32767"
)
return v
self._write(
self.AUTOMATIC_MOTOR_SPEED,
list(struct.pack("<hh", check(left, "left"), check(right, "right"))),
)
def get_automatic_motor_speed(self):
"""Get the left and right automatic motor speed as a tuple, or None if
the system is not in automatic mode."""
(left, right) = self._read(self.AUTOMATIC_MOTOR_SPEED, 4, "<hh")
return (left, right) if left != -32768 else None
def set_left_motor_speed(self, speed: float):
"""Set the left motor speed between -100 and 100."""
self.set_motor_speed(speed, None)
def set_right_motor_speed(self, speed: float):
"""Set the right motor speed between -100 and 100."""
self.set_motor_speed(None, speed)
def standby(self):
"""Stop the motors by putting the controller board in standby
mode."""
self.set_motor_speed(None, None)
def get_encoder_ticks(self) -> tuple[int, int]:
"""Retrieve the encoder ticks since the last time it was
queried. The ticks must be retrieved before they overflow a 2
byte signed integer (-32768..32767) or the result will make no
sense. Return a pair with left and right data."""
return self._read(self.ENCODER_TICKS, 4, "hh")
def get_status(self) -> dict[str, bool]:
"""Return a dict with status fields:
- "moving": True if at least one motor is moving, False otherwise
- "automatic": True if the motors are in automatic mode, False otherwise"""
(status,) = self._read(self.STATUS, 1, "B")
return {"moving": (status & 1) != 0, "automatic": (status & 2) != 0}
def set_motor_shutdown_timeout(self, duration: float):
"""Set the duration in seconds after which the motors will
shut down if no valid command is received. The minimum is 0.1
seconds, the maximum is 10 seconds."""
if duration < 0.1 or duration > 10.0:
raise ValueError
self._write(self.MOTOR_SHUTDOWN_TIMEOUT, [round(duration * 10)])
def get_motor_shutdown_timeout(self) -> float:
"""Get the duration in seconds after which the motors will shut down
if no valid command is received."""
return self._read(self.MOTOR_SHUTDOWN_TIMEOUT, 1, "B")[0] / 10
def get_firmware_version(self) -> tuple[int, int, int]:
"""Get the firmware version (major, minor, patch)."""
return self._read(self.FIRMWARE_VERSION, 3, "BBB")
def check_firmware_version(self):
"""Check that the firmware uses a version compatible with this
library."""
version = self.get_firmware_version()
Controller._check_firmware_version_consistency(
_REQUIRED_FIRMWARE_VERSION, version
)
def _check_firmware_version_consistency(
required: tuple[int, int], version: tuple[int, int, int]
):
(MAJOR, MINOR) = required
(major, minor, patch) = version
error = None
if major != MAJOR or minor < MINOR:
version = f"{major}.{minor}.{patch}"
VERSION = f"{MAJOR}.{MINOR}.*"
error = (
f"Hardware runs firmware version {version} which "
f"is not compatible with this library version ({VERSION})"
)
raise FirmwareVersionMismatch(error)
def set_pwm_frequency(self, freq: int):
"""Set the PWM frequency in Hz, between 1 and 100000."""
if freq < 1 or freq > 100000:
raise ValueError(f"PWM frequency is out of [1, 100000] range: {freq}")
self._write(
self.PWM_FREQUENCY, [freq & 0xFF, (freq >> 8) & 0xFF, (freq >> 16) & 0xFF]
)
def get_pwm_frequency(self):
"""Return the PWM frequency in Hz."""
a, b, c = self._read(self.PWM_FREQUENCY, 3, "BBB")
return a | (b << 8) | (c << 16)
def reset(self):
"""Reset the device. Used mainly for testing."""
self._write(self.RESET, [])
def reset_to_bootloader(self):
"""Reset the device to bootloader mode. Used for reprogramming."""
self._write(self.RESET_TO_BOOTLOADER, [])
def get_device_id(self):
"""Return the 8 bytes composing the device id."""
return list(self._read(self.DEVICE_ID, 8, "BBBBBBBB"))