110 lines
3.7 KiB
Python
110 lines
3.7 KiB
Python
from .Raspbot_Lib import Raspbot
|
|
|
|
# Motor ID assignments (from Raspbot_Lib comments)
|
|
_MOTOR_FL = 0 # L1 — front left
|
|
_MOTOR_RL = 1 # L2 — rear left
|
|
_MOTOR_FR = 2 # R1 — front right
|
|
_MOTOR_RR = 3 # R2 — rear right
|
|
|
|
_DRIVER_MAX = 255
|
|
|
|
|
|
class MotorController:
|
|
"""
|
|
Thin adapter between the ROS 2 motor controller node and Raspbot_Lib.
|
|
|
|
Speed convention: the node passes values in the range
|
|
[-max_speed, max_speed] (default max_speed = 1.0). This class scales
|
|
them linearly to the driver's 0-255 range, so max_speed should match
|
|
whatever normalised unit you use (1.0 → full throttle).
|
|
"""
|
|
|
|
def __init__(self, max_speed: float = 1.0):
|
|
self._max_speed = max_speed
|
|
self._bot: Raspbot | None = None
|
|
# Shadow register — Raspbot_Lib has no read-back, so we track state.
|
|
self._speeds = [0.0, 0.0, 0.0, 0.0] # [FL, FR, RL, RR]
|
|
|
|
# ------------------------------------------------------------------
|
|
# Lifecycle
|
|
# ------------------------------------------------------------------
|
|
|
|
def initialize(self) -> None:
|
|
"""Create the I2C connection and ensure all motors are stopped."""
|
|
self._bot = Raspbot()
|
|
self._stop_all_motors()
|
|
|
|
def shutdown(self) -> None:
|
|
"""Stop motors and release the driver handle."""
|
|
if self._bot is not None:
|
|
self._stop_all_motors()
|
|
self._bot = None
|
|
|
|
# ------------------------------------------------------------------
|
|
# Motor control
|
|
# ------------------------------------------------------------------
|
|
|
|
def set_speed(
|
|
self,
|
|
front_left: float,
|
|
front_right: float,
|
|
rear_left: float,
|
|
rear_right: float,
|
|
) -> None:
|
|
"""
|
|
Set all four wheel speeds simultaneously.
|
|
|
|
Parameters
|
|
----------
|
|
front_left, front_right, rear_left, rear_right:
|
|
Desired speeds in the range [-max_speed, max_speed].
|
|
Positive = forward, negative = backward.
|
|
"""
|
|
self._require_initialized()
|
|
mapping = [
|
|
(_MOTOR_FL, front_left),
|
|
(_MOTOR_RL, rear_left),
|
|
(_MOTOR_FR, front_right),
|
|
(_MOTOR_RR, rear_right),
|
|
]
|
|
for motor_id, speed in mapping:
|
|
self._bot.Ctrl_Muto(motor_id, self._to_driver_units(speed))
|
|
|
|
self._speeds = [front_left, front_right, rear_left, rear_right]
|
|
|
|
def stop_all(self) -> None:
|
|
"""Immediately stop all motors."""
|
|
self._require_initialized()
|
|
self._stop_all_motors()
|
|
self._speeds = [0.0, 0.0, 0.0, 0.0]
|
|
|
|
def get_speeds(self) -> list[float]:
|
|
"""
|
|
Return the last commanded wheel speeds as [FL, FR, RL, RR].
|
|
|
|
The Raspbot driver provides no encoder/tachometer read-back, so this
|
|
returns the most recently written values.
|
|
"""
|
|
return list(self._speeds)
|
|
|
|
# ------------------------------------------------------------------
|
|
# Helpers
|
|
# ------------------------------------------------------------------
|
|
|
|
def _to_driver_units(self, speed: float) -> int:
|
|
"""Scale a speed value to the driver's integer range [-255, 255]."""
|
|
if self._max_speed == 0:
|
|
return 0
|
|
scaled = (speed / self._max_speed) * _DRIVER_MAX
|
|
return int(max(-_DRIVER_MAX, min(_DRIVER_MAX, scaled)))
|
|
|
|
def _stop_all_motors(self) -> None:
|
|
for motor_id in (_MOTOR_FL, _MOTOR_RL, _MOTOR_FR, _MOTOR_RR):
|
|
self._bot.Ctrl_Muto(motor_id, 0)
|
|
|
|
def _require_initialized(self) -> None:
|
|
if self._bot is None:
|
|
raise RuntimeError(
|
|
"MotorController has not been initialized. Call initialize() first."
|
|
)
|