108 lines
3.6 KiB
Python
108 lines
3.6 KiB
Python
"""
|
|
A simulator for the SRv4 Motor Board.
|
|
|
|
Provides a message parser that simulates the behavior of a motor board.
|
|
|
|
Based on the Motor Board v4.4.1 firmware.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import logging
|
|
|
|
from sbot_interface.devices.motor import MAX_POWER, MIN_POWER, BaseMotor
|
|
|
|
LOGGER = logging.getLogger(__name__)
|
|
|
|
|
|
class MotorBoard:
|
|
"""
|
|
A simulator for the SRv4 Motor Board.
|
|
|
|
:param motors: A list of simulated motors connected to the motor board.
|
|
The list is indexed by the motor number.
|
|
:param asset_tag: The asset tag to report for the motor board.
|
|
:param software_version: The software version to report for the motor board.
|
|
"""
|
|
|
|
def __init__(
|
|
self,
|
|
motors: list[BaseMotor],
|
|
asset_tag: str,
|
|
software_version: str = '4.4.1'
|
|
):
|
|
self.motors = motors
|
|
self.asset_tag = asset_tag
|
|
self.software_version = software_version
|
|
|
|
def handle_command(self, command: str) -> str:
|
|
"""
|
|
Process a command string and return the response.
|
|
|
|
Executes the appropriate action on any specified motors automatically.
|
|
|
|
:param command: The command string to process.
|
|
:return: The response to the command.
|
|
"""
|
|
args = command.split(':')
|
|
if args[0] == '*IDN?':
|
|
return f'Student Robotics:MCv4B:{self.asset_tag}:{self.software_version}'
|
|
elif args[0] == '*STATUS?':
|
|
# Output faults are unsupported
|
|
return "0,0:12000"
|
|
elif args[0] == '*RESET':
|
|
LOGGER.info(f'Resetting motor board {self.asset_tag}')
|
|
for motor in self.motors:
|
|
motor.disable()
|
|
return 'ACK'
|
|
elif args[0] == 'MOT':
|
|
if len(args) < 2:
|
|
return 'NACK:Missing motor number'
|
|
|
|
try:
|
|
motor_number = int(args[1])
|
|
except ValueError:
|
|
return 'NACK:Invalid motor number'
|
|
if not (0 <= motor_number < len(self.motors)):
|
|
return 'NACK:Invalid motor number'
|
|
|
|
if len(args) < 3:
|
|
return 'NACK:Missing motor command'
|
|
if args[2] == 'SET':
|
|
if len(args) < 4:
|
|
return 'NACK:Missing motor power'
|
|
try:
|
|
power = int(args[3])
|
|
except ValueError:
|
|
return 'NACK:Invalid motor power'
|
|
if not (MIN_POWER <= power <= MAX_POWER):
|
|
return 'NACK:Invalid motor power'
|
|
LOGGER.info(
|
|
f'Setting motor {motor_number} on board {self.asset_tag} to {power}'
|
|
)
|
|
self.motors[motor_number].set_power(power)
|
|
return 'ACK'
|
|
elif args[2] == 'GET?':
|
|
return ':'.join([
|
|
f'{int(self.motors[motor_number].enabled())}',
|
|
f'{self.motors[motor_number].get_power()}',
|
|
])
|
|
elif args[2] == 'DISABLE':
|
|
LOGGER.info(f'Disabling motor {motor_number} on board {self.asset_tag}')
|
|
self.motors[motor_number].disable()
|
|
return 'ACK'
|
|
elif args[2] == 'I?':
|
|
return str(self.current())
|
|
else:
|
|
return 'NACK:Unknown motor command'
|
|
else:
|
|
return f'NACK:Unknown command {command.strip()}'
|
|
return 'NACK:Command failed'
|
|
|
|
def current(self) -> int:
|
|
"""
|
|
Get the total current draw of all motors.
|
|
|
|
:return: The total current draw of all motors in mA.
|
|
"""
|
|
return sum(motor.get_current() for motor in self.motors)
|