First commit

simulator/modules/sbot_interface/boards/__init__.py

@@ -0,0 +1,22 @@
+"""
+A set of board simulators for the SRO robot.
+
+When connected to the SocketServer, these can be used with the sr-robot3 project.
+"""
+from sbot_interface.boards.arduino import Arduino
+from sbot_interface.boards.camera import CameraBoard
+from sbot_interface.boards.led_board import LedBoard
+from sbot_interface.boards.motor_board import MotorBoard
+from sbot_interface.boards.power_board import PowerBoard
+from sbot_interface.boards.servo_board import ServoBoard
+from sbot_interface.boards.time_server import TimeServer
+
+__all__ = [
+    'Arduino',
+    'CameraBoard',
+    'LedBoard',
+    'MotorBoard',
+    'PowerBoard',
+    'ServoBoard',
+    'TimeServer',
+]

simulator/modules/sbot_interface/boards/arduino.py

@@ -0,0 +1,112 @@
+"""
+A simulator for the SR Arduino board.
+
+Provides a message parser that simulates the behavior of an Arduino board.
+
+Based on the Arduino v2.0 firmware.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.arduino_devices import BasePin, GPIOPinMode, UltrasonicSensor
+
+LOGGER = logging.getLogger(__name__)
+
+
+class Arduino:
+    """
+    A simulator for the SR Arduino board.
+
+    :param pins: A list of simulated devices connected to the Arduino board.
+                 The list is indexed by the pin number and EmptyPin is used for
+                 unconnected pins.
+    :param asset_tag: The asset tag to report for the Arduino board.
+    :param software_version: The software version to report for the Arduino board.
+    """
+
+    def __init__(self, pins: list[BasePin], asset_tag: str, software_version: str = '2'):
+        self.pins = pins
+        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 pins automatically.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        if command[0] == 'a':  # analog read
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number == -1:
+                return '0'
+            return str(self.pins[pin_number].get_analog())
+        elif command[0] == 'r':  # digital read
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number == -1:
+                return 'l'
+            return 'h' if self.pins[pin_number].get_digital() else 'l'
+        elif command[0] == 'l':  # digital write low
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_digital(False)
+            return ''
+        elif command[0] == 'h':  # digital write high
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_digital(True)
+            return ''
+        elif command[0] == 'i':  # set pin mode to input
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_mode(GPIOPinMode.INPUT)
+            return ''
+        elif command[0] == 'o':  # set pin mode to output
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_mode(GPIOPinMode.OUTPUT)
+            return ''
+        elif command[0] == 'p':  # set pin mode to input pullup
+            pin_number = self._convert_pin_number(command, 1)
+            if pin_number != -1:
+                self.pins[pin_number].set_mode(GPIOPinMode.INPUT_PULLUP)
+            return ''
+        elif command[0] == 'u':  # ultrasonic measurement
+            pulse_pin = self._convert_pin_number(command, 1)
+            echo_pin = self._convert_pin_number(command, 2)
+
+            if pulse_pin == -1 or echo_pin == -1:
+                return '0'
+
+            ultrasound_sensor = self.pins[echo_pin]
+            if isinstance(ultrasound_sensor, UltrasonicSensor):
+                return str(ultrasound_sensor.get_distance())
+            else:
+                return '0'
+        elif command[0] == 'v':  # software version
+            return f"SRduino:{self.software_version}"
+        else:
+            # A problem here: we do not know how to handle the command!
+            # Just ignore this for now.
+            return ''
+
+    def _convert_pin_number(self, command: str, index: int) -> int:
+        if len(command) < index + 1:
+            LOGGER.warning(f'Incomplete arduino command: {command}')
+            return -1
+
+        pin_str = command[index]
+        try:
+            pin_number = ord(pin_str) - ord('a')
+        except ValueError:
+            LOGGER.warning(f'Invalid pin number in command: {command}')
+            return -1
+
+        if 0 < pin_number < len(self.pins):
+            return pin_number
+        else:
+            LOGGER.warning(f'Invalid pin number in command: {command}')
+            return -1

simulator/modules/sbot_interface/boards/camera.py

@@ -0,0 +1,75 @@
+"""
+A simulator for the SRO Camera interface.
+
+Provides a message parser that simulates the behavior of a Camera.
+Interfaces to a WebotsRemoteCameraSource in the sr-robot3 package.
+"""
+from __future__ import annotations
+
+import logging
+import struct
+
+from sbot_interface.devices.camera import BaseCamera
+
+LOGGER = logging.getLogger(__name__)
+
+# *IDN?
+# *STATUS?
+# *RESET
+# CAM:CALIBRATION?
+# CAM:RESOLUTION?
+# CAM:FRAME!
+
+
+class CameraBoard:
+    """
+    A simulator for the SRO Camera interface.
+
+    :param camera: The camera object to interface with.
+    :param asset_tag: The asset tag to report for the camera board.
+    :param software_version: The software version to report for the camera board.
+    """
+
+    def __init__(self, camera: BaseCamera, asset_tag: str, software_version: str = '1.0'):
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+        self.camera = camera
+
+    def handle_command(self, command: str) -> str | bytes:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on the camera 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:CAMv1a:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            return 'ACK'
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting camera board {self.asset_tag}')
+            return 'ACK'
+        elif args[0] == 'CAM':
+            if len(args) < 2:
+                return 'NACK:Missing camera command'
+
+            if args[1] == 'CALIBRATION?':
+                LOGGER.info(f'Getting calibration data from camera on board {self.asset_tag}')
+                return ':'.join(map(str, self.camera.get_calibration()))
+            elif args[1] == 'RESOLUTION?':
+                LOGGER.info(f'Getting resolution from camera on board {self.asset_tag}')
+                resolution = self.camera.get_resolution()
+                return f'{resolution[0]}:{resolution[1]}'
+            elif args[1] == 'FRAME!':
+                LOGGER.info(f'Getting image from camera on board {self.asset_tag}')
+                resolution = self.camera.get_resolution()
+                img_len = resolution[0] * resolution[1] * 4  # 4 bytes per pixel
+                return struct.pack('>BI', 0, img_len) + self.camera.get_image()
+            else:
+                return 'NACK:Unknown camera command'
+        else:
+            return f'NACK:Unknown command {command}'
+        return 'NACK:Command failed'

simulator/modules/sbot_interface/boards/led_board.py

@@ -0,0 +1,117 @@
+"""
+A simulator for the SRO LED hat.
+
+Provides a message parser that simulates the behavior of the LED hat.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.led import RGB_COLOURS, BaseLed
+
+LOGGER = logging.getLogger(__name__)
+
+# *IDN?
+# *STATUS?
+# *RESET
+# LED:<n>:SET:<r>:<g>:<b>
+# LED:<n>:GET?
+# LED:START:SET:<0/1>
+# LED:START:GET?
+
+LED_START = 4
+
+
+class LedBoard:
+    """
+    A simulator for the SRO LED hat.
+
+    :param leds: A list of simulated LEDs connected to the LED hat.
+                    The list is indexed by the LED number.
+    :param asset_tag: The asset tag to report for the LED hat.
+    :param software_version: The software version to report for the LED hat.
+    """
+
+    def __init__(self, leds: list[BaseLed], asset_tag: str, software_version: str = '1.0'):
+        self.leds = leds
+        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 LEDs 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:KCHv1B:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            return 'ACK'
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting led board {self.asset_tag}')
+            for led in self.leds:
+                led.set_colour(0)
+            return 'ACK'
+        elif args[0] == 'LED':
+            if len(args) < 2:
+                return 'NACK:Missing LED number'
+
+            if args[1] == 'START':
+                led_number = LED_START
+                if len(args) < 3:
+                    return 'NACK:Missing LED command'
+                if args[2] == 'SET':
+                    if len(args) < 4:
+                        return 'NACK:Missing LED start'
+                    try:
+                        start = int(args[3])
+                    except ValueError:
+                        return 'NACK:Invalid LED start'
+                    if start not in [0, 1]:
+                        return 'NACK:Invalid LED start'
+                    LOGGER.info(f'Setting start LED on board {self.asset_tag} to {start}')
+                    self.leds[led_number].set_colour(start)
+                    return 'ACK'
+                elif args[2] == 'GET?':
+                    return str(self.leds[led_number].get_colour())
+                else:
+                    return "NACK:Unknown start command"
+            else:
+                try:
+                    led_number = int(args[1])
+                except ValueError:
+                    return 'NACK:Invalid LED number'
+                if not (0 <= led_number < len(self.leds)):
+                    return 'NACK:Invalid LED number'
+
+                if len(args) < 3:
+                    return 'NACK:Missing LED command'
+                if args[2] == 'SET':
+                    if len(args) < 6:
+                        return 'NACK:Missing LED colour'
+                    try:
+                        r = bool(int(args[3]))
+                        g = bool(int(args[4]))
+                        b = bool(int(args[5]))
+                    except ValueError:
+                        return 'NACK:Invalid LED colour'
+                    if r not in [0, 1] or g not in [0, 1] or b not in [0, 1]:
+                        return 'NACK:Invalid LED colour'
+                    LOGGER.info(
+                        f'Setting LED {led_number} on board {self.asset_tag} to '
+                        f'{r:d}:{g:d}:{b:d} (colour {RGB_COLOURS.index((r, g, b))}',
+                    )
+                    self.leds[led_number].set_colour(RGB_COLOURS.index((r, g, b)))
+                    return 'ACK'
+                elif args[2] == 'GET?':
+                    colour = RGB_COLOURS[self.leds[led_number].get_colour()]
+                    return f"{colour[0]:d}:{colour[1]:d}:{colour[2]:d}"
+                else:
+                    return 'NACK:Unknown LED command'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'

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)

simulator/modules/sbot_interface/boards/power_board.py

@@ -0,0 +1,192 @@
+"""
+A simulator for the SRv4 Power Board.
+
+Provides a message parser that simulates the behavior of a power board.
+
+Based on the Power Board v4.4.2 firmware.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.led import BaseLed
+from sbot_interface.devices.power import BaseButton, BaseBuzzer, Output
+
+LOGGER = logging.getLogger(__name__)
+
+NUM_OUTPUTS = 7  # 6 12V outputs, 1 5V output
+SYS_OUTPUT = 4  # L2 output for the brain
+
+RUN_LED = 0
+ERR_LED = 1
+
+
+class PowerBoard:
+    """
+    A simulator for the SRv4 Power Board.
+
+    :param outputs: A list of simulated outputs connected to the power board.
+                        The list is indexed by the output number.
+    :param buzzer: A simulated buzzer connected to the power board.
+    :param button: A simulated button connected to the power board.
+    :param leds: A tuple of simulated LEDs connected to the power board.
+    :param asset_tag: The asset tag to report for the power board.
+    :param software_version: The software version to report for the power board.
+    """
+
+    def __init__(
+        self,
+        outputs: list[Output],
+        buzzer: BaseBuzzer,
+        button: BaseButton,
+        leds: tuple[BaseLed, BaseLed],
+        asset_tag: str,
+        software_version: str = '4.4.2',
+    ):
+        self.outputs = outputs
+        self.buzzer = buzzer
+        self.button = button
+        self.leds = leds
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+        self.temp = 25
+        self.battery_voltage = 12000
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on any specified outputs, LEDs,
+        or the buzzer 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:PBv4B:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            # Output faults are unsupported, fan is always off
+            return f"0,0,0,0,0,0,0:{self.temp}:0:5000"
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting power board {self.asset_tag}')
+            for output in self.outputs:
+                output.set_output(False)
+            self.buzzer.set_note(0, 0)
+            self.leds[RUN_LED].set_colour(0)
+            self.leds[ERR_LED].set_colour(0)
+            return 'ACK'
+        elif args[0] == 'BTN':
+            if len(args) < 2:
+                return 'NACK:Missing button command'
+            if args[1] == 'START' and args[2] == 'GET?':
+                return f'{self.button.get_state():d}:0'
+            else:
+                return 'NACK:Unknown button command'
+        elif args[0] == 'OUT':
+            if len(args) < 2:
+                return 'NACK:Missing output number'
+            try:
+                output_number = int(args[1])
+            except ValueError:
+                return 'NACK:Invalid output number'
+            if not (0 <= output_number < NUM_OUTPUTS):
+                return 'NACK:Invalid output number'
+
+            if len(args) < 3:
+                return 'NACK:Missing output command'
+            if args[2] == 'SET':
+                if output_number == SYS_OUTPUT:
+                    return 'NACK:Brain output cannot be controlled'
+                if len(args) < 4:
+                    return 'NACK:Missing output state'
+                try:
+                    state = int(args[3])
+                except ValueError:
+                    return 'NACK:Invalid output state'
+                if state not in [0, 1]:
+                    return 'NACK:Invalid output state'
+                LOGGER.info(
+                    f'Setting output {output_number} on board {self.asset_tag} to {state}'
+                )
+                self.outputs[output_number].set_output(bool(state))
+                return 'ACK'
+            elif args[2] == 'GET?':
+                return '1' if self.outputs[output_number].get_output() else '0'
+            elif args[2] == 'I?':
+                return str(self.outputs[output_number].get_current())
+            else:
+                return 'NACK:Unknown output command'
+        elif args[0] == 'BATT':
+            if len(args) < 2:
+                return 'NACK:Missing battery command'
+            if args[1] == 'V?':
+                return str(self.battery_voltage)
+            elif args[1] == 'I?':
+                return str(self.current())
+            else:
+                return 'NACK:Unknown battery command'
+        elif args[0] == 'LED':
+            if len(args) < 3:
+                return 'NACK:Missing LED command'
+            if args[1] not in ['RUN', 'ERR']:
+                return 'NACK:Invalid LED type'
+
+            led_type = RUN_LED if args[1] == 'RUN' else ERR_LED
+
+            if args[2] == 'SET':
+                if len(args) < 4:
+                    return 'NACK:Missing LED state'
+                if args[3] in ['0', '1', 'F']:
+                    LOGGER.info(
+                        f'Setting {args[1]} LED on board {self.asset_tag} to {args[3]}'
+                    )
+                    if args[3] == 'F':
+                        self.leds[led_type].set_colour(1)
+                    else:
+                        self.leds[led_type].set_colour(int(args[3]))
+                    return 'ACK'
+                else:
+                    return 'NACK:Invalid LED state'
+            elif args[2] == 'GET?':
+                return str(self.leds[led_type].get_colour())
+            else:
+                return 'NACK:Invalid LED command'
+        elif args[0] == 'NOTE':
+            if len(args) < 2:
+                return 'NACK:Missing note command'
+            if args[1] == 'GET?':
+                return ':'.join(map(str, self.buzzer.get_note()))
+            else:
+                if len(args) < 3:
+                    return 'NACK:Missing note frequency'
+                try:
+                    freq = int(args[1])
+                except ValueError:
+                    return 'NACK:Invalid note frequency'
+                if not (0 <= freq < 10000):
+                    return 'NACK:Invalid note frequency'
+
+                try:
+                    dur = int(args[2])
+                except ValueError:
+                    return 'NACK:Invalid note duration'
+                if dur < 0:
+                    return 'NACK:Invalid note duration'
+
+                LOGGER.info(
+                    f'Setting buzzer on board {self.asset_tag} to {freq}Hz for {dur}ms'
+                )
+                self.buzzer.set_note(freq, dur)
+                return 'ACK'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'
+
+    def current(self) -> int:
+        """
+        Get the total current draw of all outputs.
+
+        :return: The total current draw of all outputs in mA.
+        """
+        return sum(output.get_current() for output in self.outputs)

simulator/modules/sbot_interface/boards/servo_board.py

@@ -0,0 +1,105 @@
+"""
+A simulator for the SRv4 Servo Board.
+
+Provides a message parser that simulates the behavior of a servo board.
+
+Based on the Servo Board v4.4 firmware.
+"""
+from __future__ import annotations
+
+import logging
+
+from sbot_interface.devices.servo import MAX_POSITION, MIN_POSITION, BaseServo
+
+LOGGER = logging.getLogger(__name__)
+
+
+class ServoBoard:
+    """
+    A simulator for the SRv4 Servo Board.
+
+    :param servos: A list of simulated servos connected to the servo board.
+                        The list is indexed by the servo number.
+    :param asset_tag: The asset tag to report for the servo board.
+    :param software_version: The software version to report for the servo board.
+    """
+
+    def __init__(self, servos: list[BaseServo], asset_tag: str, software_version: str = '4.4'):
+        self.servos = servos
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+        self.watchdog_fail = False
+        self.pgood = True
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        Executes the appropriate action on any specified servos 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:SBv4B:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            return f"{self.watchdog_fail:d}:{self.pgood:d}"
+        elif args[0] == '*RESET':
+            LOGGER.info(f'Resetting servo board {self.asset_tag}')
+            for servo in self.servos:
+                servo.disable()
+            return 'ACK'
+        elif args[0] == 'SERVO':
+            if len(args) < 2:
+                return 'NACK:Missing servo number'
+            if args[1] == 'I?':
+                return str(self.current())
+            elif args[1] == 'V?':
+                return '5000'
+
+            try:
+                servo_number = int(args[1])
+            except ValueError:
+                return 'NACK:Invalid servo number'
+            if not (0 <= servo_number < len(self.servos)):
+                return 'NACK:Invalid servo number'
+
+            if len(args) < 3:
+                return 'NACK:Missing servo command'
+
+            if args[2] == 'DISABLE':
+                LOGGER.info(f'Disabling servo {servo_number} on board {self.asset_tag}')
+                self.servos[servo_number].disable()
+                return 'ACK'
+            elif args[2] == 'GET?':
+                return str(self.servos[servo_number].get_position())
+            elif args[2] == 'SET':
+                if len(args) < 4:
+                    return 'NACK:Missing servo setpoint'
+
+                try:
+                    setpoint = int(args[3])
+                except ValueError:
+                    return 'NACK:Invalid servo setpoint'
+                if not (MIN_POSITION <= setpoint <= MAX_POSITION):
+                    return 'NACK:Invalid servo setpoint'
+
+                LOGGER.info(
+                    f'Setting servo {servo_number} on board {self.asset_tag} to {setpoint}'
+                )
+                self.servos[servo_number].set_position(setpoint)
+                return 'ACK'
+            else:
+                return 'NACK:Unknown servo command'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'
+
+    def current(self) -> int:
+        """
+        Get the total current draw of all servos.
+
+        :return: The total current draw of all servos in mA.
+        """
+        return sum(servo.get_current() for servo in self.servos)

simulator/modules/sbot_interface/boards/time_server.py

@@ -0,0 +1,66 @@
+"""
+A simulator for handling time based commands using simulated time.
+
+Provides a message parser that simulates the behavior of sleep and time.
+"""
+from __future__ import annotations
+
+import logging
+from datetime import datetime, timedelta
+
+from sbot_interface.devices.util import get_globals
+
+LOGGER = logging.getLogger(__name__)
+g = get_globals()
+
+
+class TimeServer:
+    """
+    A simulator for handling time based commands using simulated time.
+
+    :param asset_tag: The asset tag to report for the time server.
+    :param software_version: The software version to report for the time server.
+    :param start_time: The start time for the time server (reported time to simulator time 0).
+    """
+
+    def __init__(
+        self,
+        asset_tag: str,
+        software_version: str = '1.0',
+        start_time: str = '2024-06-01T00:00:00+00:00',
+    ):
+        self.asset_tag = asset_tag
+        self.software_version = software_version
+        self.start_time = datetime.fromisoformat(start_time)
+
+    def handle_command(self, command: str) -> str:
+        """
+        Process a command string and return the response.
+
+        :param command: The command string to process.
+        :return: The response to the command.
+        """
+        args = command.split(':')
+        if args[0] == '*IDN?':
+            return f'SourceBots:TimeServer:{self.asset_tag}:{self.software_version}'
+        elif args[0] == '*STATUS?':
+            return "Yes"
+        elif args[0] == '*RESET':
+            return "NACK:Reset not supported"
+        elif args[0] == 'TIME?':
+            sim_time = g.robot.getTime()
+            current_time = self.start_time + timedelta(seconds=sim_time)
+            return current_time.isoformat('T', timespec='milliseconds')
+        elif args[0] == 'SLEEP':
+            if len(args) < 2:
+                return 'NACK:Missing duration'
+            try:
+                duration = int(args[1])
+            except ValueError:
+                return 'NACK:Invalid duration'
+            LOGGER.info(f'Sleeping for {duration} ms')
+            g.sleep(duration / 1000)
+            return 'ACK'
+        else:
+            return f'NACK:Unknown command {command.strip()}'
+        return 'NACK:Command failed'