First commit

simulator/modules/sbot_interface/boards/motor_board.py

@@ -0,0 +1,107 @@
+"""
+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)