From 4b3572ae14fe44d39db01a0e902aaab871770929 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 18:52:02 +0100 Subject: [PATCH 01/20] Rename these types to clarify they're boards --- modules/sr/robot3/motor.py | 6 +++--- modules/sr/robot3/power.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/sr/robot3/motor.py b/modules/sr/robot3/motor.py index 911ee9de..88db2020 100644 --- a/modules/sr/robot3/motor.py +++ b/modules/sr/robot3/motor.py @@ -12,9 +12,9 @@ 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__( diff --git a/modules/sr/robot3/power.py b/modules/sr/robot3/power.py index 9e06c9a7..2f1b8772 100644 --- a/modules/sr/robot3/power.py +++ b/modules/sr/robot3/power.py @@ -15,11 +15,11 @@ class Outputs(Enum): OUT_L3 = 'L3' -def init_power_board(webot: Robot) -> Power: - return Power() +def init_power_board(webot: Robot) -> PowerBoard: + return PowerBoard() -class Power: +class PowerBoard: def __init__(self) -> None: self.outputs = OutputGroup() self.battery_sensor = BatterySensor() From b8650c59e4dbffda2637d6604b2ab5d6a53becf8 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 17:23:25 +0100 Subject: [PATCH 02/20] Bump minimum Python version This actually changed back in 21ba150, but this check was missed. --- modules/sr/robot3/_version_check.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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." ) From a8ef27c20a5071e2ae5844ff348d1c202a5f5ecb Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 17:01:52 +0100 Subject: [PATCH 03/20] Add print_wifi_details Not because this does anything useful, but rather because adding this is easy for compatbility. --- modules/sr/robot3/robot.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/modules/sr/robot3/robot.py b/modules/sr/robot3/robot.py index 88f9df39..d44a74b2 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -88,6 +88,9 @@ def webots_step_and_should_continue(self, duration_ms: int) -> bool: 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" From 9acd33a37024bb5bb3c41f14707fe22d2aceb298 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 17:22:44 +0100 Subject: [PATCH 04/20] AnaloguePin should be an IntEnum --- modules/sr/robot3/ruggeduino.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/modules/sr/robot3/ruggeduino.py b/modules/sr/robot3/ruggeduino.py index c01d9644..18e38c1d 100644 --- a/modules/sr/robot3/ruggeduino.py +++ b/modules/sr/robot3/ruggeduino.py @@ -1,6 +1,6 @@ from __future__ import annotations -from enum import Enum +from enum import IntEnum from typing import Dict, Union from controller import Robot @@ -12,13 +12,13 @@ INPUT_PULLUP = 2 -class AnaloguePin(Enum): - A0 = "A0" - A1 = "A1" - A2 = "A2" - A3 = "A3" - A4 = "A4" - A5 = "A5" +class AnaloguePin(IntEnum): + A0 = 14 + A1 = 15 + A2 = 16 + A3 = 17 + A4 = 18 + A5 = 19 ARDUINO_DEVICES_TYPE = Dict[Union[AnaloguePin, int], Union[DistanceSensor, Microswitch, Led]] From 60e507e745dddd0efeb28bdccce574787803e3e6 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 17:28:11 +0100 Subject: [PATCH 05/20] Update power board outputs --- modules/sr/robot3/__init__.py | 4 ++-- modules/sr/robot3/power.py | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/modules/sr/robot3/__init__.py b/modules/sr/robot3/__init__.py index 25650a54..b5fc1f66 100644 --- a/modules/sr/robot3/__init__.py +++ b/modules/sr/robot3/__init__.py @@ -14,8 +14,8 @@ 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 @@ -43,8 +43,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/power.py b/modules/sr/robot3/power.py index 2f1b8772..8d8bc569 100644 --- a/modules/sr/robot3/power.py +++ b/modules/sr/robot3/power.py @@ -11,8 +11,9 @@ 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) -> PowerBoard: From d49ed1cfaf163c7cc300fb526d394c977b3f2d68 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 17:41:53 +0100 Subject: [PATCH 06/20] Update the buzzer, mostly to support blocking calls The other change here is the name of the 'note' -> 'pitch' argument. --- modules/sr/robot3/power.py | 33 +++++++++++++++++++++++++-------- modules/sr/robot3/robot.py | 2 +- 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/modules/sr/robot3/power.py b/modules/sr/robot3/power.py index 8d8bc569..21e564b4 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): @@ -16,15 +18,15 @@ class Outputs(Enum): OUT_FIVE_VOLT = 'FIVE_VOLT' -def init_power_board(webot: Robot) -> PowerBoard: - return PowerBoard() +def init_power_board(robot: Robot) -> PowerBoard: + return PowerBoard(robot) class PowerBoard: - def __init__(self) -> None: + def __init__(self, robot: Robot) -> None: self.outputs = OutputGroup() self.battery_sensor = BatterySensor() - self.piezo = Piezo() + self.piezo = Piezo(robot) class OutputGroup: @@ -82,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 d44a74b2..f97d1b22 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -143,7 +143,7 @@ def _init_devs(self) -> None: # No camera for SR2021 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) From 9da1e4df4647ce8845a4c0e1b6410fd5bd2e41f6 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 17:58:08 +0100 Subject: [PATCH 07/20] Align ruggeduino control modes with sr.robot3 We only expose those which actually work in the simulator, though this gets the rest of them present internally should we want them. --- modules/sr/robot3/__init__.py | 7 ++++++- modules/sr/robot3/ruggeduino.py | 16 +++++++++++++--- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/modules/sr/robot3/__init__.py b/modules/sr/robot3/__init__.py index b5fc1f66..90101504 100644 --- a/modules/sr/robot3/__init__.py +++ b/modules/sr/robot3/__init__.py @@ -8,7 +8,7 @@ MARKER_TOKEN_GOLD, MARKER_TOKEN_SILVER, ) -from sr.robot3.ruggeduino import INPUT, OUTPUT, AnaloguePin, INPUT_PULLUP +from sr.robot3.ruggeduino import AnaloguePin, GPIOPinMode OUT_H0 = Outputs.OUT_H0 OUT_H1 = Outputs.OUT_H1 @@ -25,6 +25,11 @@ A5 = AnaloguePin.A5 +OUTPUT = GPIOPinMode.DIGITAL_OUTPUT +INPUT = GPIOPinMode.DIGITAL_INPUT +INPUT_PULLUP = GPIOPinMode.DIGITAL_INPUT_PULLUP + + __all__ = ( 'OUTPUT', 'INPUT', diff --git a/modules/sr/robot3/ruggeduino.py b/modules/sr/robot3/ruggeduino.py index 18e38c1d..06e6caa0 100644 --- a/modules/sr/robot3/ruggeduino.py +++ b/modules/sr/robot3/ruggeduino.py @@ -7,9 +7,19 @@ from sr.robot3.ruggeduino_devices import Led, Microswitch, DistanceSensor 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.""" + + 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. + + PWM_OUTPUT = 6 #: A PWM output signal can be created on the pin. class AnaloguePin(IntEnum): From 33ebe329d69d351941299a46d5a80b4ea6791473 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 18:27:02 +0100 Subject: [PATCH 08/20] Handle singular boards the same way sr.robot3 does --- modules/sr/robot3/robot.py | 28 ++++++++++++++++++++++------ 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/modules/sr/robot3/robot.py b/modules/sr/robot3/robot.py index f97d1b22..580d74bb 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -3,12 +3,15 @@ import math import random from os import path, environ +from typing import TypeVar, Collection from threading import Lock from sr.robot3 import motor, power, servos, ruggeduino # Webots specific library from controller import Robot as WebotsRobot +T = TypeVar('T') + class Robot: """ @@ -147,18 +150,31 @@ def _init_power_board(self) -> None: 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] 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] 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] + + 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 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') def time(self) -> float: """ From 8e9fa5f5d9d5916e99762fac227dba390c555e60 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 18:27:39 +0100 Subject: [PATCH 09/20] Connect up the camera, coping with there not being one on the robot --- modules/sr/robot3/camera.py | 13 ++++++++++--- modules/sr/robot3/robot.py | 13 +++++++++++-- stubs/controller.py | 2 +- 3 files changed, 22 insertions(+), 6 deletions(-) diff --git a/modules/sr/robot3/camera.py b/modules/sr/robot3/camera.py index 3a8594cd..2540f7c6 100644 --- a/modules/sr/robot3/camera.py +++ b/modules/sr/robot3/camera.py @@ -5,7 +5,7 @@ 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 @@ -129,11 +129,11 @@ 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) @@ -181,3 +181,10 @@ def _see(self) -> list[Marker]: markers.append(Marker(face, marker_info, when)) return markers + + +def init_cameras(webot: Robot, lock: threading.Lock) -> list[Camera]: + camera = webot.getCamera('camera') + if camera is None: + return [] + return [Camera(webot, camera, lock)] diff --git a/modules/sr/robot3/robot.py b/modules/sr/robot3/robot.py index 580d74bb..badd729f 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -6,7 +6,7 @@ from typing import TypeVar, Collection from threading import Lock -from sr.robot3 import motor, power, servos, ruggeduino +from sr.robot3 import motor, power, camera, servos, ruggeduino # Webots specific library from controller import Robot as WebotsRobot @@ -143,7 +143,8 @@ 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) @@ -157,6 +158,10 @@ def _init_servos(self) -> None: def _init_ruggeduinos(self) -> None: 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: @@ -164,6 +169,10 @@ def _singular(self, elements: Collection[T], name: str) -> T: 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') diff --git a/stubs/controller.py b/stubs/controller.py index 062cd679..e446cee0 100644 --- a/stubs/controller.py +++ b/stubs/controller.py @@ -285,7 +285,7 @@ def getBasicTimeStep(self) -> float: ... def getCustomData(self) -> str: ... def setCustomData(self, data: str) -> None: ... - def getCamera(self, name: str) -> Camera: ... + def getCamera(self, name: str) -> Camera | None: ... def getDistanceSensor(self, name: str) -> DistanceSensor: ... def getEmitter(self, name: str) -> Emitter: ... def getLED(self, name: str) -> LED: ... From d6c7c52f66f417fa995ec04822ad100f408130a1 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 19:15:57 +0100 Subject: [PATCH 10/20] Don't use deprecated getDevice variants --- modules/sr/robot3/camera.py | 4 +++- modules/sr/robot3/utils.py | 9 +++++++++ stubs/controller.py | 11 ++--------- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/modules/sr/robot3/camera.py b/modules/sr/robot3/camera.py index 2540f7c6..a4b6c172 100644 --- a/modules/sr/robot3/camera.py +++ b/modules/sr/robot3/camera.py @@ -9,6 +9,8 @@ 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}$") @@ -184,7 +186,7 @@ def _see(self) -> list[Marker]: def init_cameras(webot: Robot, lock: threading.Lock) -> list[Camera]: - camera = webot.getCamera('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/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 e446cee0..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 | None: ... - 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: ... From c9e3d65a0248a388efdc154278323e3d9cef1372 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sat, 8 Oct 2022 19:27:52 +0100 Subject: [PATCH 11/20] Update the camera to match the sr.robot3 signatures This doesn't update the markers, which will be much more involved to update. --- modules/sr/robot3/camera.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/modules/sr/robot3/camera.py b/modules/sr/robot3/camera.py index a4b6c172..3ce3135e 100644 --- a/modules/sr/robot3/camera.py +++ b/modules/sr/robot3/camera.py @@ -141,7 +141,7 @@ def __init__(self, webot: Robot, camera: WebotCamera, lock: threading.Lock) -> N 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. @@ -184,6 +184,15 @@ def _see(self) -> list[Marker]: 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) From a0e491e33e84dbff9d44c8c4d7cf83971e2fba19 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sun, 16 Oct 2022 14:44:09 +0100 Subject: [PATCH 12/20] Add metadata equivalent to sr.robot3 --- modules/sr/robot3/__init__.py | 5 +++ modules/sr/robot3/metadata.py | 66 +++++++++++++++++++++++++++++++++++ modules/sr/robot3/robot.py | 53 +++++++++++++++------------- 3 files changed, 100 insertions(+), 24 deletions(-) create mode 100644 modules/sr/robot3/metadata.py diff --git a/modules/sr/robot3/__init__.py b/modules/sr/robot3/__init__.py index 90101504..d4e8ea5d 100644 --- a/modules/sr/robot3/__init__.py +++ b/modules/sr/robot3/__init__.py @@ -8,6 +8,7 @@ MARKER_TOKEN_GOLD, MARKER_TOKEN_SILVER, ) +from sr.robot3.metadata import RobotMode from sr.robot3.ruggeduino import AnaloguePin, GPIOPinMode OUT_H0 = Outputs.OUT_H0 @@ -24,6 +25,8 @@ A4 = AnaloguePin.A4 A5 = AnaloguePin.A5 +COMP = RobotMode.COMP +DEV = RobotMode.DEV OUTPUT = GPIOPinMode.DIGITAL_OUTPUT INPUT = GPIOPinMode.DIGITAL_INPUT @@ -36,6 +39,8 @@ 'INPUT_PULLUP', 'COAST', 'BRAKE', + 'COMP', + 'DEV', 'Robot', 'Note', 'A0', 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/robot.py b/modules/sr/robot3/robot.py index badd729f..a4e9d44f 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -2,11 +2,11 @@ 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, camera, servos, ruggeduino +from sr.robot3 import motor, power, camera, servos, metadata, ruggeduino # Webots specific library from controller import Robot as WebotsRobot @@ -29,10 +29,7 @@ def __init__(self, auto_start: bool = False, verbose: bool = True) -> None: # returns a float, but should always actually be an integer value 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 @@ -50,12 +47,11 @@ def init(self) -> None: 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() - - return 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() @@ -97,17 +93,6 @@ def print_wifi_details(self) -> None: 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 @@ -117,7 +102,7 @@ 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') while ( @@ -185,6 +170,26 @@ def ruggeduino(self) -> ruggeduino.Ruggeduino: 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. From 6379eb75a70f2e91c9419349cdf82a231f1da9e9 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sun, 16 Oct 2022 22:15:48 +0100 Subject: [PATCH 13/20] Update the Robot constructor signature to match sr.robot3 --- modules/sr/robot3/robot.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/modules/sr/robot3/robot.py b/modules/sr/robot3/robot.py index a4e9d44f..9fd01ee5 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -21,7 +21,20 @@ class Robot: manually by calling the `sleep` method. """ - def __init__(self, auto_start: bool = False, verbose: bool = True) -> None: + 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._initialised = False self._quiet = not verbose From 5aac01b08770bb969885272d17b39a4c4f49567c Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sun, 16 Oct 2022 22:15:57 +0100 Subject: [PATCH 14/20] No need for a separate init method --- modules/sr/robot3/robot.py | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/modules/sr/robot3/robot.py b/modules/sr/robot3/robot.py index 9fd01ee5..33fc694f 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -35,7 +35,6 @@ def __init__( Note: `env` and `ignored_ruggeduinos` are ignored in the simulator. """ - self._initialised = False self._quiet = not verbose self.webot = WebotsRobot() @@ -50,15 +49,12 @@ def __init__( # 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() + if not auto_start: + self.wait_start() + def _get_user_code_info(self) -> str | None: user_version_path = self._code_path / '.user-rev' try: From b52c2d8a93bc368903c9e3e8076c76cc1442fbfd Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sun, 16 Oct 2022 22:20:57 +0100 Subject: [PATCH 15/20] Make the Webot robot instance private Now we're trying a bit harder to match the sr.robot3 API this makes a bit more sense to be private. It also makes it a bit less likely that competitors will find/use it. --- modules/sr/robot3/robot.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/modules/sr/robot3/robot.py b/modules/sr/robot3/robot.py index 33fc694f..3f5102ee 100644 --- a/modules/sr/robot3/robot.py +++ b/modules/sr/robot3/robot.py @@ -37,9 +37,9 @@ def __init__( 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._metadata, self._code_path = metadata.init_metadata() @@ -93,7 +93,7 @@ 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: @@ -113,9 +113,9 @@ def wait_start(self) -> None: 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 @@ -144,17 +144,17 @@ def _init_power_board(self) -> None: self.power_board = power.init_power_board(self) def _init_motors(self) -> None: - self.motor_boards = motor.init_motor_array(self.webot) + self.motor_boards = motor.init_motor_array(self._webot) def _init_servos(self) -> None: - self.servo_boards = servos.init_servo_board(self.webot) + self.servo_boards = servos.init_servo_board(self._webot) def _init_ruggeduinos(self) -> None: - self.ruggeduinos = ruggeduino.init_ruggeduino_array(self.webot) + 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) + self._cameras = camera.init_cameras(self._webot, self._step_lock) def _singular(self, elements: Collection[T], name: str) -> T: num = len(elements) @@ -203,7 +203,7 @@ 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: """ From d24a242bb0d006f44fb5e3c21bab25f6ba7c85d3 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sun, 16 Oct 2022 23:10:05 +0100 Subject: [PATCH 16/20] Simplify this type annotation --- modules/sr/robot3/ruggeduino.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/modules/sr/robot3/ruggeduino.py b/modules/sr/robot3/ruggeduino.py index 06e6caa0..2d64e346 100644 --- a/modules/sr/robot3/ruggeduino.py +++ b/modules/sr/robot3/ruggeduino.py @@ -4,7 +4,12 @@ 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 @@ -31,7 +36,7 @@ class AnaloguePin(IntEnum): A5 = 19 -ARDUINO_DEVICES_TYPE = Dict[Union[AnaloguePin, int], Union[DistanceSensor, Microswitch, Led]] +ARDUINO_DEVICES_TYPE = Dict[Union[AnaloguePin, int], RuggeduinoDevice] def init_ruggeduino_array(webot: Robot) -> dict[str, Ruggeduino]: From 725efd482e80511419785e8b4cbb7c5d5e58d1a5 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Sun, 16 Oct 2022 23:10:45 +0100 Subject: [PATCH 17/20] Name this type in the more usual style --- modules/sr/robot3/ruggeduino.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/sr/robot3/ruggeduino.py b/modules/sr/robot3/ruggeduino.py index 2d64e346..6e96a995 100644 --- a/modules/sr/robot3/ruggeduino.py +++ b/modules/sr/robot3/ruggeduino.py @@ -36,7 +36,7 @@ class AnaloguePin(IntEnum): A5 = 19 -ARDUINO_DEVICES_TYPE = Dict[Union[AnaloguePin, int], RuggeduinoDevice] +DevicesMapping = Dict[Union[AnaloguePin, int], RuggeduinoDevice] def init_ruggeduino_array(webot: Robot) -> dict[str, Ruggeduino]: @@ -63,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) } @@ -72,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) @@ -87,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, @@ -110,6 +110,6 @@ class Ruggeduino: def __init__( self, - devices: ARDUINO_DEVICES_TYPE, + devices: DevicesMapping, ) -> None: self.pins = devices From ee314ea2bb85ce1e51847b24ee4dd9961f1cd139 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Mon, 17 Oct 2022 22:22:37 +0100 Subject: [PATCH 18/20] Reject rather than coercing invalid power/position values This matches sr.robot3. --- modules/sr/robot3/motor.py | 11 +++++------ modules/sr/robot3/servos.py | 7 +++++++ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/modules/sr/robot3/motor.py b/modules/sr/robot3/motor.py index 88db2020..3d188302 100644 --- a/modules/sr/robot3/motor.py +++ b/modules/sr/robot3/motor.py @@ -6,7 +6,7 @@ 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 @@ -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/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, From 38e13c57cb4e9119595292074db3dd7baeeeb592 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Tue, 18 Oct 2022 19:38:34 +0100 Subject: [PATCH 19/20] Add missing trailing comma --- modules/sr/robot3/power.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/sr/robot3/power.py b/modules/sr/robot3/power.py index 21e564b4..d7012ea1 100644 --- a/modules/sr/robot3/power.py +++ b/modules/sr/robot3/power.py @@ -92,7 +92,7 @@ def buzz( duration: int | float | datetime.timedelta, pitch: Pitch, *, - blocking: bool | None = None + blocking: bool | None = None, ) -> None: if not blocking: return From a324d6abd1154ba5f81951d0c6aac21032a56046 Mon Sep 17 00:00:00 2001 From: Peter Law Date: Tue, 18 Oct 2022 22:42:51 +0100 Subject: [PATCH 20/20] Teach the keyboard controller sr.robot3's motor scale is smaller --- .../example_controller/keyboard_controller.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) 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