diff --git a/controllers/example_controller/keyboard_controller.py b/controllers/example_controller/keyboard_controller.py index feee2d4a..87065a95 100644 --- a/controllers/example_controller/keyboard_controller.py +++ b/controllers/example_controller/keyboard_controller.py @@ -80,20 +80,20 @@ def print_sensors(robot: Robot) -> None: boost = True if key_ascii == key_forward: - left_power += 50 - right_power += 50 + left_power += 0.5 + right_power += 0.5 elif key_ascii == key_reverse: - left_power += -50 - right_power += -50 + left_power += -0.5 + right_power += -0.5 elif key_ascii == key_left: - left_power -= 25 - right_power += 25 + left_power -= 0.25 + right_power += 0.25 elif key_ascii == key_right: - left_power += 25 - right_power -= 25 + left_power += 0.25 + right_power -= 0.25 elif key_ascii == key_sense: print_sensors(R) @@ -111,9 +111,9 @@ def print_sensors(robot: Robot) -> None: key = keyboard.getKey() if boost: - # double power values but constrain to [-100, 100] - left_power = max(min(left_power * 2, 100), -100) - right_power = max(min(right_power * 2, 100), -100) + # double power values but constrain to [-1, 1] + left_power = max(min(left_power * 2, 1), -1) + right_power = max(min(right_power * 2, 1), -1) R.motor_board.motors[0].power = left_power R.motor_board.motors[1].power = right_power diff --git a/modules/sr/robot3/__init__.py b/modules/sr/robot3/__init__.py index 25650a54..d4e8ea5d 100644 --- a/modules/sr/robot3/__init__.py +++ b/modules/sr/robot3/__init__.py @@ -8,14 +8,15 @@ MARKER_TOKEN_GOLD, MARKER_TOKEN_SILVER, ) -from sr.robot3.ruggeduino import INPUT, OUTPUT, AnaloguePin, INPUT_PULLUP +from sr.robot3.metadata import RobotMode +from sr.robot3.ruggeduino import AnaloguePin, GPIOPinMode OUT_H0 = Outputs.OUT_H0 OUT_H1 = Outputs.OUT_H1 OUT_L0 = Outputs.OUT_L0 OUT_L1 = Outputs.OUT_L1 -OUT_L2 = Outputs.OUT_L2 OUT_L3 = Outputs.OUT_L3 +OUT_FIVE_VOLT = Outputs.OUT_FIVE_VOLT A0 = AnaloguePin.A0 A1 = AnaloguePin.A1 @@ -24,6 +25,13 @@ A4 = AnaloguePin.A4 A5 = AnaloguePin.A5 +COMP = RobotMode.COMP +DEV = RobotMode.DEV + +OUTPUT = GPIOPinMode.DIGITAL_OUTPUT +INPUT = GPIOPinMode.DIGITAL_INPUT +INPUT_PULLUP = GPIOPinMode.DIGITAL_INPUT_PULLUP + __all__ = ( 'OUTPUT', @@ -31,6 +39,8 @@ 'INPUT_PULLUP', 'COAST', 'BRAKE', + 'COMP', + 'DEV', 'Robot', 'Note', 'A0', @@ -43,8 +53,8 @@ 'OUT_H1', 'OUT_L0', 'OUT_L1', - 'OUT_L2', 'OUT_L3', + 'OUT_FIVE_VOLT', 'MarkerType', 'MARKER_ARENA', 'MARKER_TOKEN_GOLD', diff --git a/modules/sr/robot3/_version_check.py b/modules/sr/robot3/_version_check.py index 60516a68..4f429399 100644 --- a/modules/sr/robot3/_version_check.py +++ b/modules/sr/robot3/_version_check.py @@ -4,7 +4,7 @@ # Provide a nicer check that the user has the right version of Python than # them getting a `SyntaxError` -assert sys.version_info >= (3, 7), ( +assert sys.version_info >= (3, 8), ( "Sorry, you must be using a recent version of Python 3. " "Please see the SR docs for how to switch your Python version." ) diff --git a/modules/sr/robot3/camera.py b/modules/sr/robot3/camera.py index 3a8594cd..3ce3135e 100644 --- a/modules/sr/robot3/camera.py +++ b/modules/sr/robot3/camera.py @@ -5,10 +5,12 @@ from enum import Enum from typing import NamedTuple -from controller import Robot +from controller import Robot, Camera as WebotCamera from sr.robot3.vision import Face, Orientation, tokens_from_objects from sr.robot3.coordinates import Point +from .utils import maybe_get_robot_device + MARKER_MODEL_RE = re.compile(r"^[AGS]\d{0,2}$") @@ -129,17 +131,17 @@ def orientation(self) -> Orientation: class Camera: - def __init__(self, webot: Robot, lock: threading.Lock) -> None: + def __init__(self, webot: Robot, camera: WebotCamera, lock: threading.Lock) -> None: self._webot = webot self._timestep = int(webot.getBasicTimeStep()) - self.camera = webot.getCamera("camera") + self.camera = camera self.camera.enable(self._timestep) self.camera.recognitionEnable(self._timestep) self._lock = lock - def see(self) -> list[Marker]: + def see(self, *, eager: bool = True) -> list[Marker]: """ Identify items which the camera can see and return a list of `Marker` instances describing them. @@ -181,3 +183,19 @@ def _see(self) -> list[Marker]: markers.append(Marker(face, marker_info, when)) return markers + + def see_ids(self) -> list[int]: + # While in theory this method ought to be the "fast" method, processing + # speed doesn't matter much in the simulator and with the locking we + # need to do it's much easier to let this be a shallow wrapper around + # the full implementation. + return [x.info.code for x in self.see()] + + # The simulator does not emulate the `capture` or `save` methods. + + +def init_cameras(webot: Robot, lock: threading.Lock) -> list[Camera]: + camera = maybe_get_robot_device(webot, 'camera', WebotCamera) + if camera is None: + return [] + return [Camera(webot, camera, lock)] diff --git a/modules/sr/robot3/metadata.py b/modules/sr/robot3/metadata.py new file mode 100644 index 00000000..eaffdc8d --- /dev/null +++ b/modules/sr/robot3/metadata.py @@ -0,0 +1,66 @@ +from __future__ import annotations + +import os +import enum +import dataclasses +from pathlib import Path + +_ETC_OS_RELEASE = Path('/etc/os-release') + + +class RobotMode(enum.Enum): + # Note: The simulator internally continues to use the historical (lower + # case) spelling of these modes, however for the competitor-facing API we + # provide uppercased spellings to match sr.robot3. + + COMP = 'COMP' + DEV = 'DEV' + + +@dataclasses.dataclass(frozen=True) +class Metadata: + """Minimal version of robot metadata.""" + + arena: str = 'A' + zone: int = 0 + mode: RobotMode = RobotMode.DEV + marker_offset: int = 0 + game_timeout: int | None = None + wifi_enabled: bool = True + + # From Software + astoria_version: str = '' + kernel_version: str = '' + arch: str = '' + python_version: str = '' + libc_ver: str = '' + os_name: str | None = None + os_pretty_name: str | None = None + os_version: str | None = None + + # From robot settings file + usercode_entrypoint: str = '' + wifi_ssid: str | None = None + wifi_psk: str | None = None + wifi_region: str | None = None + + def is_wifi_valid(self) -> bool: + return True + + @classmethod + def get_os_version_info(cls, os_release_path: Path = _ETC_OS_RELEASE) -> dict[str, str]: + return {} + + +def init_metadata() -> tuple[Metadata, Path]: + mode_str = os.environ.get('SR_ROBOT_MODE', 'dev').upper() + + zone_str = os.environ.get('SR_ROBOT_ZONE', '0') + zone = int(zone_str) + if zone not in range(4): + raise ValueError(f"Zone must be in range 0-3 inclusive. {zone_str!r} is invalid") + + return ( + Metadata(mode=RobotMode(mode_str), zone=zone), + Path(os.environ['SR_ROBOT_FILE']).parent.absolute(), + ) diff --git a/modules/sr/robot3/motor.py b/modules/sr/robot3/motor.py index 911ee9de..3d188302 100644 --- a/modules/sr/robot3/motor.py +++ b/modules/sr/robot3/motor.py @@ -6,15 +6,15 @@ from sr.robot3.motor_devices import Wheel, Gripper, LinearMotor # The maximum value that the motor board will accept -SPEED_MAX = 1.0 +SPEED_MAX = 1 COAST = 0 BRAKE = 0 -def init_motor_array(webot: Robot) -> dict[str, Motor]: +def init_motor_array(webot: Robot) -> dict[str, MotorBoard]: return { - 'srABC1': Motor( + 'srABC1': MotorBoard( Wheel(webot, 'left wheel'), Wheel(webot, 'right wheel'), ), @@ -36,7 +36,7 @@ def translate(sr_speed_val: float, sr_motor: Gripper | Wheel | LinearMotor) -> f ) -class Motor: +class MotorBoard: """Represents a motor board.""" def __init__( @@ -77,11 +77,10 @@ def power(self, value: float) -> None: "target setter function" self._power = value - # Limit the value to within the valid range - if value > SPEED_MAX: - value = SPEED_MAX - elif value < -SPEED_MAX: - value = -SPEED_MAX + if value > SPEED_MAX or value < -SPEED_MAX: + raise ValueError( + f"Motor power must be between {SPEED_MAX} and -{SPEED_MAX}.", + ) if self.sr_motor: self.sr_motor.set_speed(translate(value, self.sr_motor)) diff --git a/modules/sr/robot3/power.py b/modules/sr/robot3/power.py index 9e06c9a7..d7012ea1 100644 --- a/modules/sr/robot3/power.py +++ b/modules/sr/robot3/power.py @@ -1,9 +1,11 @@ from __future__ import annotations +import datetime from enum import Enum -from typing import Union, Iterator +from typing import Union, Iterator, TYPE_CHECKING -from controller import Robot +if TYPE_CHECKING: + from .robot import Robot class Outputs(Enum): @@ -11,19 +13,20 @@ class Outputs(Enum): OUT_H1 = 'H1' OUT_L0 = 'L0' OUT_L1 = 'L1' - OUT_L2 = 'L2' + # OUT_L2 = 'L2' # Brain board runs from L2 OUT_L3 = 'L3' + OUT_FIVE_VOLT = 'FIVE_VOLT' -def init_power_board(webot: Robot) -> Power: - return Power() +def init_power_board(robot: Robot) -> PowerBoard: + return PowerBoard(robot) -class Power: - def __init__(self) -> None: +class PowerBoard: + def __init__(self, robot: Robot) -> None: self.outputs = OutputGroup() self.battery_sensor = BatterySensor() - self.piezo = Piezo() + self.piezo = Piezo(robot) class OutputGroup: @@ -81,8 +84,23 @@ def current(self) -> float: class Piezo: - def buzz(self, duration: float, note: Pitch) -> None: - pass + def __init__(self, robot: Robot) -> None: + self.robot = robot + + def buzz( + self, + duration: int | float | datetime.timedelta, + pitch: Pitch, + *, + blocking: bool | None = None, + ) -> None: + if not blocking: + return + + if isinstance(duration, datetime.timedelta): + duration = duration.total_seconds() + + self.robot.sleep(duration) class Note(float, Enum): diff --git a/modules/sr/robot3/robot.py b/modules/sr/robot3/robot.py index 88f9df39..3f5102ee 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -2,13 +2,16 @@ import math import random -from os import path, environ +from typing import TypeVar, Collection +from pathlib import Path from threading import Lock -from sr.robot3 import motor, power, servos, ruggeduino +from sr.robot3 import motor, power, camera, servos, metadata, ruggeduino # Webots specific library from controller import Robot as WebotsRobot +T = TypeVar('T') + class Robot: """ @@ -18,18 +21,27 @@ class Robot: manually by calling the `sleep` method. """ - def __init__(self, auto_start: bool = False, verbose: bool = True) -> None: - self._initialised = False + def __init__( + self, + *, + auto_start: bool = False, + verbose: bool = False, + env: object = None, + ignored_ruggeduinos: list[str] | None = None, + ) -> None: + """ + Initialise robot. + + Note: `env` and `ignored_ruggeduinos` are ignored in the simulator. + """ + self._quiet = not verbose - self.webot = WebotsRobot() + self._webot = WebotsRobot() # returns a float, but should always actually be an integer value - self._timestep = int(self.webot.getBasicTimeStep()) + self._timestep = int(self._webot.getBasicTimeStep()) - self.mode = environ.get("SR_ROBOT_MODE", "dev") - self.zone = int(environ.get("SR_ROBOT_ZONE", 0)) - self.arena = "A" - self.usbkey = path.normpath(path.join(environ["SR_ROBOT_FILE"], "../")) + self._metadata, self._code_path = metadata.init_metadata() # Lock used to guard access to Webot's time stepping machinery, allowing # us to safely advance simulation time from *either* the competitor's @@ -37,22 +49,18 @@ def __init__(self, auto_start: bool = False, verbose: bool = True) -> None: # thread, but not both. self._step_lock = Lock() - self.init() - if not auto_start: - self.wait_start() - - def init(self) -> None: self._init_devs() - self._initialised = True self.display_info() - def _get_user_code_info(self) -> str | None: - user_version_path = path.join(self.usbkey, '.user-rev') - if path.exists(user_version_path): - with open(user_version_path) as f: - return f.read().strip() + if not auto_start: + self.wait_start() - return None + def _get_user_code_info(self) -> str | None: + user_version_path = self._code_path / '.user-rev' + try: + return user_version_path.read_text().strip() + except IOError: + return None def display_info(self) -> None: user_code_version = self._get_user_code_info() @@ -85,23 +93,15 @@ def webots_step_and_should_continue(self, duration_ms: int) -> bool: # `synchronization` is left at its default value of `TRUE`). In # that mode, Webots returns -1 from step to indicate that the # simulation is terminating, or 0 otherwise. - result = self.webot.step(duration_ms) + result = self._webot.step(duration_ms) return result != -1 + def print_wifi_details(self) -> None: + print("The simulated robot does not have WiFi.") # noqa: T201 + def wait_start(self) -> None: "Wait for the start signal to happen" - if self.mode not in ["comp", "dev"]: - raise Exception( - "mode of '%s' is not supported -- must be 'comp' or 'dev'" % self.mode, - ) - if self.zone < 0 or self.zone > 3: - raise Exception( - "zone must be in range 0-3 inclusive -- value of %i is invalid" % self.zone, - ) - if self.arena not in ["A", "B"]: - raise Exception("arena must be A or B") - print("Waiting for start signal.") # noqa: T201 # Always advance time by a little bit. This simulates the real-world @@ -111,11 +111,11 @@ def wait_start(self) -> None: self._timestep * random.randint(8, 20), ) - if self.mode == 'comp': + if self.mode == metadata.RobotMode.COMP: # Interact with the supervisor "robot" to wait for the start of the match. - self.webot.setCustomData('ready') + self._webot.setCustomData('ready') while ( - self.webot.getCustomData() != 'start' and + self._webot.getCustomData() != 'start' and self.webots_step_and_should_continue(self._timestep) ): pass @@ -137,31 +137,73 @@ def _init_devs(self) -> None: # Ruggeduinos self._init_ruggeduinos() - # No camera for SR2021 + # Camera + self._init_cameras() def _init_power_board(self) -> None: - self.power_board = power.init_power_board(self.webot) + self.power_board = power.init_power_board(self) def _init_motors(self) -> None: - self.motor_boards = motor.init_motor_array(self.webot) - if len(self.motor_boards) == 1: - self.motor_board = list(self.motor_boards.values())[0] + self.motor_boards = motor.init_motor_array(self._webot) def _init_servos(self) -> None: - self.servo_boards = servos.init_servo_board(self.webot) - if len(self.servo_boards) == 1: - self.servo_board = list(self.servo_boards.values())[0] + self.servo_boards = servos.init_servo_board(self._webot) def _init_ruggeduinos(self) -> None: - self.ruggeduinos = ruggeduino.init_ruggeduino_array(self.webot) - if len(self.ruggeduinos) == 1: - self.ruggeduino = list(self.ruggeduinos.values())[0] + self.ruggeduinos = ruggeduino.init_ruggeduino_array(self._webot) + + def _init_cameras(self) -> None: + # See comment in Camera.see for why we need to pass the step lock here. + self._cameras = camera.init_cameras(self._webot, self._step_lock) + + def _singular(self, elements: Collection[T], name: str) -> T: + num = len(elements) + if num != 1: + raise ValueError(f"Expected exactly one {name} to be connected, but found {num}") + x, = elements + return x + + @property + def camera(self) -> camera.Camera: + return self._singular(self._cameras, 'camera') + + @property + def motor_board(self) -> motor.MotorBoard: + return self._singular(self.motor_boards.values(), 'motor board') + + @property + def ruggeduino(self) -> ruggeduino.Ruggeduino: + return self._singular(self.ruggeduinos.values(), 'ruggeduino') + + @property + def servo_board(self) -> servos.ServoBoard: + return self._singular(self.servo_boards.values(), 'servo board') + + @property + def arena(self) -> str: + return self.metadata.arena + + @property + def mode(self) -> metadata.RobotMode: + return self.metadata.mode + + @property + def usbkey(self) -> Path | None: + return self._code_path + + @property + def zone(self) -> int: + return self.metadata.zone + + @property + def metadata(self) -> metadata.Metadata: + return self._metadata def time(self) -> float: """ Roughly equivalent to `time.time` but for simulation time. """ - return self.webot.getTime() + return self._webot.getTime() def sleep(self, secs: float) -> None: """ diff --git a/modules/sr/robot3/ruggeduino.py b/modules/sr/robot3/ruggeduino.py index c01d9644..6e96a995 100644 --- a/modules/sr/robot3/ruggeduino.py +++ b/modules/sr/robot3/ruggeduino.py @@ -1,27 +1,42 @@ from __future__ import annotations -from enum import Enum +from enum import IntEnum from typing import Dict, Union from controller import Robot -from sr.robot3.ruggeduino_devices import Led, Microswitch, DistanceSensor +from sr.robot3.ruggeduino_devices import ( + Led, + Microswitch, + DistanceSensor, + RuggeduinoDevice, +) from sr.robot3.output_frequency_limiter import OutputFrequencyLimiter -OUTPUT = 0 -INPUT = 1 -INPUT_PULLUP = 2 +class GPIOPinMode(IntEnum): + """Hardware modes that a GPIO pin can be set to.""" -class AnaloguePin(Enum): - A0 = "A0" - A1 = "A1" - A2 = "A2" - A3 = "A3" - A4 = "A4" - A5 = "A5" + DIGITAL_INPUT = 0 #: The digital state of the pin can be read + DIGITAL_INPUT_PULLUP = 1 #: Same as DIGITAL_INPUT but internal pull-up is enabled + DIGITAL_INPUT_PULLDOWN = 2 #: Same as DIGITAL_INPUT but internal pull-down is enabled + DIGITAL_OUTPUT = 3 #: The digital state of the pin can be set. + ANALOGUE_INPUT = 4 #: The analogue voltage of the pin can be read. + ANALOGUE_OUTPUT = 5 #: The analogue voltage of the pin can be set using a DAC. -ARDUINO_DEVICES_TYPE = Dict[Union[AnaloguePin, int], Union[DistanceSensor, Microswitch, Led]] + PWM_OUTPUT = 6 #: A PWM output signal can be created on the pin. + + +class AnaloguePin(IntEnum): + A0 = 14 + A1 = 15 + A2 = 16 + A3 = 17 + A4 = 18 + A5 = 19 + + +DevicesMapping = Dict[Union[AnaloguePin, int], RuggeduinoDevice] def init_ruggeduino_array(webot: Robot) -> dict[str, Ruggeduino]: @@ -48,7 +63,7 @@ def init_ruggeduino_array(webot: Robot) -> dict[str, Ruggeduino]: DistanceSensor(webot, name) for name in dist_sensor_names ] - analogue_input_dict: ARDUINO_DEVICES_TYPE = { + analogue_input_dict: DevicesMapping = { key: sensor for key, sensor in zip(AnaloguePin, analogue_sensors) } @@ -57,7 +72,7 @@ def init_ruggeduino_array(webot: Robot) -> dict[str, Ruggeduino]: Microswitch(webot, name) for name in switch_names ] - digital_input_dict: ARDUINO_DEVICES_TYPE = { + digital_input_dict: DevicesMapping = { index: sensor for index, sensor in enumerate(digital_sensors, start=Ruggeduino.DIGITAL_PIN_START) @@ -72,7 +87,7 @@ def init_ruggeduino_array(webot: Robot) -> dict[str, Ruggeduino]: ) ] - digital_output_dict: ARDUINO_DEVICES_TYPE = { + digital_output_dict: DevicesMapping = { index: output for index, output in enumerate( digital_outputs, @@ -95,6 +110,6 @@ class Ruggeduino: def __init__( self, - devices: ARDUINO_DEVICES_TYPE, + devices: DevicesMapping, ) -> None: self.pins = devices diff --git a/modules/sr/robot3/servos.py b/modules/sr/robot3/servos.py index 305f4090..48d1cb4a 100644 --- a/modules/sr/robot3/servos.py +++ b/modules/sr/robot3/servos.py @@ -3,6 +3,8 @@ from controller import Motor, Robot from sr.robot3.utils import map_to_range, get_robot_device +SERVO_LIMIT = 1 + def init_servo_board(webot: Robot) -> dict[str, ServoBoard]: return { @@ -43,6 +45,11 @@ def __init__(self, webot: Robot, servo_name: str) -> None: self.min_position = self.webot_motor.getMinPosition() def set_position(self, position: float) -> None: + if position > SERVO_LIMIT or position < -SERVO_LIMIT: + raise ValueError( + f"Servo position must be between {SERVO_LIMIT} and -{SERVO_LIMIT}.", + ) + self.webot_motor.setPosition(map_to_range( -1, 1, diff --git a/modules/sr/robot3/utils.py b/modules/sr/robot3/utils.py index bf576732..107558a4 100644 --- a/modules/sr/robot3/utils.py +++ b/modules/sr/robot3/utils.py @@ -18,6 +18,15 @@ def map_to_range( return ((value - old_min) / (old_max - old_min)) * (new_max - new_min) + new_min +def maybe_get_robot_device(robot: Robot, name: str, kind: type[TDevice]) -> TDevice | None: + device = robot.getDevice(name) + if device is None: + return None + if not isinstance(device, kind): + raise TypeError + return device + + def get_robot_device(robot: Robot, name: str, kind: type[TDevice]) -> TDevice: device = robot.getDevice(name) if not isinstance(device, kind): diff --git a/stubs/controller.py b/stubs/controller.py index 062cd679..2f6690f6 100644 --- a/stubs/controller.py +++ b/stubs/controller.py @@ -285,15 +285,8 @@ def getBasicTimeStep(self) -> float: ... def getCustomData(self) -> str: ... def setCustomData(self, data: str) -> None: ... - def getCamera(self, name: str) -> Camera: ... - def getDistanceSensor(self, name: str) -> DistanceSensor: ... - def getEmitter(self, name: str) -> Emitter: ... - def getLED(self, name: str) -> LED: ... - def getMotor(self, name: str) -> Motor: ... - def getReceiver(self, name: str) -> Receiver: ... - def getTouchSensor(self, name: str) -> TouchSensor: ... - def getCompass(self, name: str) -> Compass: ... - def getDisplay(self, name: str) -> Display: ... + # Various type-specific getThing methods exist but are deprecated. Remove + # them from the stub to prevent use. def getDevice(self, name: str) -> Device | None: ...