First commit

simulator/modules/sbot_interface/devices/__init__.py

@@ -0,0 +1 @@
+"""A set of wrappers for board simulators to interact with Webots devices."""

simulator/modules/sbot_interface/devices/arduino_devices.py

@@ -0,0 +1,275 @@
+"""A collection of wrappers for the devices that can be connected to the Arduino."""
+from abc import ABC, abstractmethod
+from enum import Enum
+
+from sbot_interface.devices.led import Led as _Led
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
+
+ANALOG_MAX = 1023
+
+
+class GPIOPinMode(str, Enum):
+    """The possible modes for a GPIO pin."""
+
+    INPUT = 'INPUT'
+    INPUT_PULLUP = 'INPUT_PULLUP'
+    OUTPUT = 'OUTPUT'
+
+
+class BasePin(ABC):
+    """The base class for all the devices that can be connected to the Arduino."""
+
+    @abstractmethod
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        pass
+
+    @abstractmethod
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        pass
+
+    @abstractmethod
+    def get_digital(self) -> bool:
+        """Get the digital input value of the pin."""
+        pass
+
+    @abstractmethod
+    def set_digital(self, value: bool) -> None:
+        """Set the digital output value of the pin."""
+        pass
+
+    @abstractmethod
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin."""
+        pass
+
+
+class EmptyPin(BasePin):
+    """A pin that does nothing. Used for pins that are not connected."""
+
+    def __init__(self) -> None:
+        self._mode = GPIOPinMode.INPUT
+        self._digital = False
+        self._analog = 0
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """Get the digital input value of the pin."""
+        return self._digital
+
+    def set_digital(self, value: bool) -> None:
+        """Set the digital output value of the pin."""
+        self._digital = value
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin."""
+        return self._analog
+
+
+class UltrasonicSensor(BasePin):
+    """
+    A sensor that can measure the distance to an object.
+
+    This is attached to the pin specified to be the echo pin, with the trigger pin unused.
+    """
+
+    def __init__(self, device_name: str) -> None:
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.DistanceSensor)
+        self._device.enable(g.timestep)
+        self._mode = GPIOPinMode.INPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """Get the digital input value of the pin. This is always False."""
+        return False
+
+    def set_digital(self, value: bool) -> None:
+        """
+        Set the digital output value of the pin.
+
+        This has no effect here.
+        """
+        pass
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin. This is always 0."""
+        return 0
+
+    def get_distance(self) -> int:
+        """
+        Get the distance measured by the sensor in mm.
+
+        Relies on the lookup table mapping to the distance in mm.
+        """
+        return int(self._device.getValue())
+
+
+class MicroSwitch(BasePin):
+    """A simple switch that can be pressed or released."""
+
+    def __init__(self, device_name: str) -> None:
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.TouchSensor)
+        self._device.enable(g.timestep)
+        self._mode = GPIOPinMode.INPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """Get the digital input value of the pin."""
+        return bool(self._device.getValue())
+
+    def set_digital(self, value: bool) -> None:
+        """
+        Set the digital output value of the pin.
+
+        This has no effect here.
+        """
+        pass
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin, either 0 or 1023."""
+        return 1023 if self.get_digital() else 0
+
+
+class PressureSensor(BasePin):
+    """
+    A sensor that can measure the force applied to it.
+
+    This is attached to the pin specified, with the force proportional to the analog value.
+    """
+
+    # Use lookupTable [0 0 0, 50 1023 0] // 50 Newton max force
+    def __init__(self, device_name: str) -> None:
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.TouchSensor)
+        self._device.enable(g.timestep)
+        self._mode = GPIOPinMode.INPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """
+        Get the digital input value of the pin.
+
+        True when the force is above half the maximum value.
+        """
+        return self.get_analog() > ANALOG_MAX / 2
+
+    def set_digital(self, value: bool) -> None:
+        """
+        Set the digital output value of the pin.
+
+        This has no effect here.
+        """
+        pass
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin. This is proportional to the force applied."""
+        return int(self._device.getValue())
+
+
+class ReflectanceSensor(BasePin):
+    """
+    A simple sensor that can detect the reflectance of a surface.
+
+    Used for line following, with a higher value indicating a lighter surface.
+    """
+
+    def __init__(self, device_name: str) -> None:
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.DistanceSensor)
+        self._device.enable(g.timestep)
+        self._mode = GPIOPinMode.INPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """
+        Get the digital input value of the pin.
+
+        True when the reflectance is above half the maximum value.
+        """
+        return self.get_analog() > ANALOG_MAX / 2
+
+    def set_digital(self, value: bool) -> None:
+        """
+        Set the digital output value of the pin.
+
+        This has no effect here.
+        """
+        pass
+
+    def get_analog(self) -> int:
+        """
+        Get the analog input value of the pin.
+
+        This is proportional to the reflectance of the surface.
+        """
+        return int(self._device.getValue())
+
+
+class Led(BasePin):
+    """A simple LED that can be turned on or off."""
+
+    def __init__(self, device_name: str) -> None:
+        self._led = _Led(device_name)
+        self._mode = GPIOPinMode.OUTPUT
+
+    def get_mode(self) -> GPIOPinMode:
+        """Get the current mode of the pin."""
+        return self._mode
+
+    def set_mode(self, mode: GPIOPinMode) -> None:
+        """Set the mode of the pin."""
+        self._mode = mode
+
+    def get_digital(self) -> bool:
+        """
+        Get the digital input value of the pin.
+
+        True when the LED is on.
+        """
+        return self._led.get_colour() > 0
+
+    def set_digital(self, value: bool) -> None:
+        """Set the digital output value of the pin. This turns the LED on or off."""
+        self._led.set_colour(1 if value else 0)
+
+    def get_analog(self) -> int:
+        """Get the analog input value of the pin. This is always 0."""
+        return 0

simulator/modules/sbot_interface/devices/camera.py

@@ -0,0 +1,105 @@
+"""A wrapper for the Webots camera device."""
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from functools import lru_cache
+from math import tan
+
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
+
+g = get_globals()
+
+
+class BaseCamera(ABC):
+    """Base class for camera devices."""
+
+    @abstractmethod
+    def get_image(self) -> bytes:
+        """Get a frame from the camera, encoded as a byte string."""
+        pass
+
+    @abstractmethod
+    def get_resolution(self) -> tuple[int, int]:
+        """Get the resolution of the camera in pixels, width x height."""
+        pass
+
+    @abstractmethod
+    def get_calibration(self) -> tuple[float, float, float, float]:
+        """Return the intrinsic camera calibration parameters fx, fy, cx, cy."""
+        pass
+
+
+class NullCamera(BaseCamera):
+    """
+    Null camera device.
+
+    Allows the robot to run without a camera device attached.
+    """
+
+    def get_image(self) -> bytes:
+        """Get a frame from the camera, encoded as a byte string."""
+        return b''
+
+    def get_resolution(self) -> tuple[int, int]:
+        """Get the resolution of the camera in pixels, width x height."""
+        return 0, 0
+
+    def get_calibration(self) -> tuple[float, float, float, float]:
+        """Return the intrinsic camera calibration parameters fx, fy, cx, cy."""
+        return 0, 0, 0, 0
+
+
+# Camera
+class Camera(BaseCamera):
+    """
+    A wrapper for the Webots camera device.
+
+    The camera will sleep for 1 frame time before capturing an image to ensure the
+    image is up to date.
+
+    :param device_name: The name of the camera device.
+    :param frame_rate: The frame rate of the camera in frames per second.
+    """
+
+    def __init__(self, device_name: str, frame_rate: int) -> None:
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.Camera)
+        # round down to the nearest timestep
+        self.sample_time = int(((1000 / frame_rate) // g.timestep) * g.timestep)
+
+    def get_image(self) -> bytes:
+        """
+        Get a frame from the camera, encoded as a byte string.
+
+        Sleeps for 1 frame time before capturing the image to ensure the image is up to date.
+
+        NOTE The image data buffer is automatically freed at the end of the timestep,
+        so must not be accessed after any sleep.
+
+        :return: The image data as a byte string.
+        """
+        # A frame is only captured every sample_time milliseconds the camera is enabled
+        # so we need to wait for a frame to be captured after enabling the camera.
+        # The image data buffer is automatically freed at the end of the timestep.
+        self._device.enable(self.sample_time)
+        g.sleep(self.sample_time / 1000)
+
+        image_data_raw = self._device.getImage()
+        # Disable the camera to save computation
+        self._device.disable()  # type: ignore[no-untyped-call]
+
+        return image_data_raw
+
+    @lru_cache
+    def get_resolution(self) -> tuple[int, int]:
+        """Get the resolution of the camera in pixels, width x height."""
+        return self._device.getWidth(), self._device.getHeight()
+
+    @lru_cache
+    def get_calibration(self) -> tuple[float, float, float, float]:
+        """Return the intrinsic camera calibration parameters fx, fy, cx, cy."""
+        return (
+            (self._device.getWidth() / 2) / tan(self._device.getFov() / 2),  # fx
+            (self._device.getWidth() / 2) / tan(self._device.getFov() / 2),  # fy
+            self._device.getWidth() // 2,  # cx
+            self._device.getHeight() // 2,  # cy
+        )

simulator/modules/sbot_interface/devices/led.py

@@ -0,0 +1,88 @@
+"""
+A wrapper for the Webots LED device.
+
+Supports both single and multi-colour non-PWM LEDs.
+"""
+from abc import ABC, abstractmethod
+
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
+
+RGB_COLOURS = [
+    (False, False, False),  # OFF
+    (True, False, False),  # RED
+    (True, True, False),  # YELLOW
+    (False, True, False),  # GREEN
+    (False, True, True),  # CYAN
+    (False, False, True),  # BLUE
+    (True, False, True),  # MAGENTA
+    (True, True, True),  # WHITE
+]
+
+
+class BaseLed(ABC):
+    """Base class for LED devices."""
+
+    def __init__(self) -> None:
+        self.colour = 0
+
+    @abstractmethod
+    def set_colour(self, colour: int) -> None:
+        """Set the colour of the LED."""
+        pass
+
+    @abstractmethod
+    def get_colour(self) -> int:
+        """Get the colour of the LED."""
+        pass
+
+
+class NullLed(BaseLed):
+    """Null LED device. Allows the robot to run without an LED device attached."""
+
+    def __init__(self) -> None:
+        self.colour = 0
+
+    def set_colour(self, colour: int) -> None:
+        """Set the colour of the LED."""
+        self.colour = colour
+
+    def get_colour(self) -> int:
+        """Get the colour of the LED."""
+        return self.colour
+
+
+class Led(BaseLed):
+    """
+    A wrapper for the Webots LED device.
+
+    :param device_name: The name of the LED device.
+    :param num_colours: The number of colours the LED supports.
+    """
+
+    def __init__(self, device_name: str, num_colours: int = 1) -> None:
+        g = get_globals()
+        self.num_colours = num_colours
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.LED)
+
+    def set_colour(self, colour: int) -> None:
+        """
+        Set the colour of the LED.
+
+        :param colour: The colour to set the LED to. A 1-based index for the lookup
+                       table of the LED. 0 is OFF.
+        """
+        if 0 <= colour < self.num_colours:
+            # NOTE: value 0 is OFF
+            self._device.set(colour)
+        else:
+            raise ValueError(f'Invalid colour: {colour}')
+
+    def get_colour(self) -> int:
+        """
+        Get the colour of the LED.
+
+        :return: The colour of the LED. A 1-based index for the lookup table of the LED.
+                 0 is OFF.
+        """
+        # webots uses 1-based indexing
+        return self._device.get()

simulator/modules/sbot_interface/devices/motor.py

@@ -0,0 +1,155 @@
+"""
+A wrapper for the Webots motor device.
+
+The motor will apply a small amount of variation to the power setting to simulate
+inaccuracies in the motor.
+"""
+import logging
+from abc import ABC, abstractmethod
+
+from sbot_interface.devices.util import (
+    WebotsDevice,
+    add_jitter,
+    get_globals,
+    get_robot_device,
+    map_to_range,
+)
+
+MAX_POWER = 1000
+MIN_POWER = -1000
+
+
+class BaseMotor(ABC):
+    """Base class for motor devices."""
+
+    @abstractmethod
+    def disable(self) -> None:
+        """Disable the motor."""
+        pass
+
+    @abstractmethod
+    def set_power(self, value: int) -> None:
+        """Set the power of the motor (±1000)."""
+        pass
+
+    @abstractmethod
+    def get_power(self) -> int:
+        """Get the power of the motor."""
+        pass
+
+    @abstractmethod
+    def get_current(self) -> int:
+        """Get the current draw of the motor in mA."""
+        pass
+
+    @abstractmethod
+    def enabled(self) -> bool:
+        """Check if the motor is enabled."""
+        pass
+
+
+class NullMotor(BaseMotor):
+    """Null motor device. Allows the robot to run without a motor device attached."""
+
+    def __init__(self) -> None:
+        self.power = 0
+        self._enabled = False
+
+    def disable(self) -> None:
+        """Disable the motor."""
+        self._enabled = False
+
+    def set_power(self, value: int) -> None:
+        """Set the power of the motor."""
+        self.power = value
+        self._enabled = True
+
+    def get_power(self) -> int:
+        """Get the power of the motor."""
+        return self.power
+
+    def get_current(self) -> int:
+        """Get the current draw of the motor in mA."""
+        return 0
+
+    def enabled(self) -> bool:
+        """Check if the motor is enabled."""
+        return self._enabled
+
+
+class Motor(BaseMotor):
+    """
+    A wrapper for the Webots motor device.
+
+    The motor will apply a small amount of variation to the power setting to simulate
+    inaccuracies in the motor.
+
+    :param device_name: The name of the motor device.
+    """
+
+    def __init__(self, device_name: str) -> None:
+        self.power = 0
+        self._enabled = False
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.Motor)
+        # Put the motor in velocity control mode
+        self._device.setPosition(float('inf'))
+        self._device.setVelocity(0)
+        self._max_speed = self._device.getMaxVelocity()
+        # Limit the torque the motor can apply to have realistic acceleration
+        self._device.setAvailableTorque(1)
+
+    def disable(self) -> None:
+        """Disable the motor."""
+        self._device.setVelocity(0)
+        self._enabled = False
+
+    def set_power(self, value: int) -> None:
+        """
+        Set the power of the motor.
+
+        :param value: The power setting for the motor. A value between -1000 and 1000.
+        """
+        if value != 0:
+            if abs(value) < 0.05:
+                logging.warning(
+                    "Motor power is too low, values below 0.05 will not move the motor."
+                )
+                value = 0
+            else:
+                # Apply a small amount of variation to the power setting to simulate
+                # inaccuracies in the motor
+                value = int(add_jitter(value, (MIN_POWER, MAX_POWER)))
+
+        self._device.setVelocity(map_to_range(
+            value,
+            (MIN_POWER, MAX_POWER),
+            (-self._max_speed, self._max_speed),
+        ))
+        self.power = value
+        self._enabled = True
+
+    def get_power(self) -> int:
+        """
+        Get the power of the motor.
+
+        :return: The power setting for the motor. A value between -1000 and 1000.
+        """
+        return self.power
+
+    def get_current(self) -> int:
+        """
+        Get the current draw of the motor in mA.
+
+        :return: The current draw of the motor in mA.
+        """
+        # TODO calculate from torque feedback
+        return 0
+
+    def enabled(self) -> bool:
+        """
+        Check if the motor is enabled.
+
+        :return: True if the motor is enabled, False otherwise.
+        """
+        return self._enabled

simulator/modules/sbot_interface/devices/power.py

@@ -0,0 +1,137 @@
+"""A module to define the power devices used in the simulator."""
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from typing import Callable
+
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
+
+
+class Output:
+    """
+    A class to represent a power output.
+
+    This does not actually represent any device in the simulator,
+    but is used to simulate how the outputs on the power board would behave.
+
+    :param downstream_current: A function to get the current draw of the downstream device.
+    """
+
+    def __init__(self, downstream_current: Callable[[], int] | None = None) -> None:
+        self._enabled = False
+        self._current_func = downstream_current
+
+    def set_output(self, enable: bool) -> None:
+        """Set the output state."""
+        self._enabled = enable
+
+    def get_output(self) -> bool:
+        """Get the output state."""
+        return self._enabled
+
+    def get_current(self) -> int:
+        """Get the current draw of the output in mA."""
+        if self._current_func is not None:
+            return self._current_func()
+        return 0
+
+
+class ConnectorOutput(Output):
+    """
+    A class to represent a power output that controls a webots connector device.
+
+    :param device_name: The name of the device in webots.
+    :param downstream_current: A function to get the current draw of the downstream device.
+    """
+
+    def __init__(
+        self,
+        device_name: str,
+        downstream_current: Callable[[], int] | None = None,
+    ) -> None:
+        super().__init__(downstream_current)
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.Connector)
+        self._enabled = False
+
+    def set_output(self, enable: bool) -> None:
+        """Set the output state."""
+        if enable:
+            self._device.lock()  # type: ignore[no-untyped-call]
+        else:
+            self._device.unlock()  # type: ignore[no-untyped-call]
+
+    def get_output(self) -> bool:
+        """Get the output state."""
+        return self._device.isLocked()
+
+
+class BaseBuzzer(ABC):
+    """The base class for the buzzer device."""
+
+    @abstractmethod
+    def set_note(self, freq: int, dur: int) -> None:
+        """Set the note to play and its duration."""
+        pass
+
+    @abstractmethod
+    def get_note(self) -> tuple[int, int]:
+        """Get the note that is currently playing and its duration."""
+        pass
+
+
+class BaseButton(ABC):
+    """The base class for the button device."""
+
+    @abstractmethod
+    def get_state(self) -> bool:
+        """Get whether the button is pressed."""
+        pass
+
+
+class NullBuzzer(BaseBuzzer):
+    """A buzzer that does nothing. Used for buzzers that are not connected."""
+
+    def __init__(self) -> None:
+        self.frequency = 0
+        self.duration = 0
+        super().__init__()
+
+    def set_note(self, freq: int, dur: int) -> None:
+        """Set the note to play."""
+        self.frequency = freq
+        self.duration = dur
+
+    def get_note(self) -> tuple[int, int]:
+        """Get the note that is currently playing and its duration."""
+        return self.frequency, self.duration
+
+
+class NullButton(BaseButton):
+    """A button that does nothing. Used for buttons that are not connected."""
+
+    def get_state(self) -> bool:
+        """Return whether the button is pressed. Always returns True."""
+        # button is always pressed
+        return True
+
+
+class StartButton(BaseButton):
+    """
+    A button to represent the start button on the robot.
+
+    Uses the robot's custom data to determine if the robot is ready to start.
+    """
+
+    def __init__(self) -> None:
+        self._initialized = False
+
+    def get_state(self) -> bool:
+        """Return whether the start button is pressed."""
+        g = get_globals()
+        if not self._initialized:
+            if g.robot.getCustomData() != 'start':
+                g.robot.setCustomData('ready')
+            self._initialized = True
+
+        return bool(g.robot.getCustomData() == 'start')

simulator/modules/sbot_interface/devices/servo.py

@@ -0,0 +1,157 @@
+"""
+A wrapper for the Webots servo device.
+
+The servo will apply a small amount of variation to the power setting to simulate
+inaccuracies in the servo.
+"""
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from typing import TYPE_CHECKING
+
+from sbot_interface.devices.util import (
+    WebotsDevice,
+    add_jitter,
+    get_globals,
+    get_robot_device,
+    map_to_range,
+)
+
+if TYPE_CHECKING:
+    from controller import PositionSensor
+
+MAX_POSITION = 4000
+MIN_POSITION = 500
+SERVO_MAX = 2000
+SERVO_MIN = 1000
+
+
+class BaseServo(ABC):
+    """The base class for all the servos that can be connected to the Servo board."""
+
+    @abstractmethod
+    def disable(self) -> None:
+        """Disable the servo."""
+        pass
+
+    @abstractmethod
+    def set_position(self, value: int) -> None:
+        """
+        Set the position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        pass
+
+    @abstractmethod
+    def get_position(self) -> int:
+        """Return the current position of the servo."""
+        pass
+
+    @abstractmethod
+    def get_current(self) -> int:
+        """Return the current draw of the servo in mA."""
+        pass
+
+    @abstractmethod
+    def enabled(self) -> bool:
+        """Return whether the servo is enabled."""
+        pass
+
+
+class NullServo(BaseServo):
+    """A servo that does nothing. Used for servos that are not connected."""
+
+    def __init__(self) -> None:
+        self.position = 1500
+        self._enabled = False
+
+    def disable(self) -> None:
+        """Disable the servo."""
+        self._enabled = False
+
+    def set_position(self, value: int) -> None:
+        """
+        Set the position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        self.position = value
+        self._enabled = True
+
+    def get_position(self) -> int:
+        """
+        Return the current position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        return self.position
+
+    def get_current(self) -> int:
+        """Return the current draw of the servo in mA."""
+        return 0
+
+    def enabled(self) -> bool:
+        """Return whether the servo is enabled."""
+        return self._enabled
+
+
+class Servo(BaseServo):
+    """A servo connected to the Servo board."""
+
+    def __init__(self, device_name: str) -> None:
+        self.position = (SERVO_MAX + SERVO_MIN) // 2
+        # TODO use setAvailableForce to simulate disabled
+        self._enabled = False
+        g = get_globals()
+        self._device = get_robot_device(g.robot, device_name, WebotsDevice.Motor)
+        self._pos_sensor: PositionSensor | None = self._device.getPositionSensor()  # type: ignore[no-untyped-call]
+        self._max_position = self._device.getMaxPosition()
+        self._min_position = self._device.getMinPosition()
+        if self._pos_sensor is not None:
+            self._pos_sensor.enable(g.timestep)
+
+    def disable(self) -> None:
+        """Disable the servo."""
+        self._enabled = False
+
+    def set_position(self, value: int) -> None:
+        """
+        Set the position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        # Apply a small amount of variation to the power setting to simulate
+        # inaccuracies in the servo
+        value = int(add_jitter(value, (SERVO_MIN, SERVO_MAX), std_dev_percent=0.5))
+
+        self._device.setPosition(map_to_range(
+            value,
+            (SERVO_MIN, SERVO_MAX),
+            (self._min_position + 0.001, self._max_position - 0.001),
+        ))
+        self.position = value
+        self._enabled = True
+
+    def get_position(self) -> int:
+        """
+        Return the current position of the servo.
+
+        Position is the pulse width in microseconds.
+        """
+        if self._pos_sensor is not None:
+            self.position = int(map_to_range(
+                self._pos_sensor.getValue(),
+                (self._min_position + 0.001, self._max_position - 0.001),
+                (MIN_POSITION, MAX_POSITION),
+            ))
+        return self.position
+
+    def get_current(self) -> int:
+        """Return the current draw of the servo in mA."""
+        # TODO calculate from torque feedback
+        return 0
+
+    def enabled(self) -> bool:
+        """Return whether the servo is enabled."""
+        return self._enabled

simulator/modules/sbot_interface/devices/util.py

@@ -0,0 +1,159 @@
+"""Utility functions for the devices module."""
+from __future__ import annotations
+
+import threading
+from dataclasses import dataclass
+from math import ceil
+from random import gauss
+from typing import TypeVar
+
+from controller import (
+    GPS,
+    LED,
+    Accelerometer,
+    Camera,
+    Compass,
+    Connector,
+    DistanceSensor,
+    Emitter,
+    Gyro,
+    InertialUnit,
+    Lidar,
+    LightSensor,
+    Motor,
+    PositionSensor,
+    Radar,
+    RangeFinder,
+    Receiver,
+    Robot,
+    Speaker,
+    TouchSensor,
+    VacuumGripper,
+)
+from controller.device import Device
+
+TDevice = TypeVar('TDevice', bound=Device)
+__GLOBALS: 'GlobalData' | None = None
+
+
+class WebotsDevice:
+    """
+    A collection of Webots device classes.
+
+    Each class represents a different device that can be attached to the robot.
+    """
+
+    Accelerometer = Accelerometer
+    Camera = Camera
+    Compass = Compass
+    Connector = Connector
+    DistanceSensor = DistanceSensor
+    Emitter = Emitter
+    GPS = GPS
+    Gyro = Gyro
+    InertialUnit = InertialUnit
+    LED = LED
+    Lidar = Lidar
+    LightSensor = LightSensor
+    Motor = Motor
+    PositionSensor = PositionSensor
+    Radar = Radar
+    RangeFinder = RangeFinder
+    Receiver = Receiver
+    Speaker = Speaker
+    TouchSensor = TouchSensor
+    VacuumGripper = VacuumGripper
+
+
+@dataclass
+class GlobalData:
+    """
+    Global data and functions for the simulator.
+
+    When accessed through the get_globals function, a single instance of this
+    class is created and stored in the module's global scope.
+
+    :param robot: The robot object.
+    :param timestep: The timestep size of the simulation.
+    :param stop_event: The event to stop the simulation.
+    """
+
+    robot: Robot
+    timestep: int
+    stop_event: threading.Event | None = None
+
+    def sleep(self, secs: float) -> None:
+        """Sleeps for a given duration in simulator time."""
+        if secs == 0:
+            return
+        elif secs < 0:
+            raise ValueError("Sleep duration must be non-negative.")
+
+        # Convert to a multiple of the timestep
+        msecs = ceil((secs * 1000) / self.timestep) * self.timestep
+
+        # Sleep for the given duration
+        result = self.robot.step(msecs)
+
+        # If the simulation has stopped, set the stop event
+        if (result == -1) and (self.stop_event is not None):
+            self.stop_event.set()
+
+
+def get_globals() -> GlobalData:
+    """Returns the global dictionary."""
+    global __GLOBALS
+    if __GLOBALS is None:
+        # Robot constructor lacks a return type annotation in R2023b
+        robot = Robot() if Robot.created is None else Robot.created  # type: ignore[no-untyped-call]
+
+        __GLOBALS = GlobalData(
+            robot=robot,
+            timestep=int(robot.getBasicTimeStep()),
+        )
+    return __GLOBALS
+
+
+def map_to_range(
+    value: float,
+    old_min_max: tuple[float, float],
+    new_min_max: tuple[float, float],
+) -> float:
+    """Maps a value from within one range of inputs to within a range of outputs."""
+    old_min, old_max = old_min_max
+    new_min, new_max = new_min_max
+    return ((value - old_min) / (old_max - old_min)) * (new_max - new_min) + new_min
+
+
+def get_robot_device(robot: Robot, name: str, kind: type[TDevice]) -> TDevice:
+    """
+    A helper function to get a device from the robot.
+
+    Raises a TypeError if the device is not found or is not of the correct type.
+    Weboots normally just returns None if the device is not found.
+
+    :param robot: The robot object.
+    :param name: The name of the device.
+    :param kind: The type of the device.
+    :return: The device object.
+    :raises TypeError: If the device is not found or is not of the correct type.
+    """
+    device = robot.getDevice(name)
+    if not isinstance(device, kind):
+        raise TypeError(f"Failed to get device: {name}.")
+    return device
+
+
+def add_jitter(
+    value: float,
+    value_range: tuple[float, float],
+    std_dev_percent: float = 2.0,
+    offset_percent: float = 0.0,
+) -> float:
+    """Adds normally distributed jitter to a given value."""
+    std_dev = value * (std_dev_percent / 100.0)
+    mean_offset = value * (offset_percent / 100.0)
+
+    error = gauss(mean_offset, std_dev)
+    # Ensure the error is within the range
+    return max(value_range[0], min(value_range[1], value + error))