Merge pull request #1 from srobo/SR2025

Updates for SR2025 game

assets/user_readme.md

@@ -168,11 +168,12 @@ The robot has a number of boards attached to it that can be interacted with usin
 These boards include:
 
 - Power Board (serial number: `PWR`)
+    - OUT_H0 controls the vacuum pump. Enabling this allows the robot to pick up the token.
 - Motor Board (serial number: `MOT`)
     - The left wheel is connected to motor 0.
     - The right wheel is connected to motor 1.
 - Servo Board (serial number: `SERVO`)
-    - No servos are attached to the simulated robot.
+    - Servo 0 controls the lifter. Setting the position to -1 will move the lifter to the bottom position and a position of 1 will move the lifter to the top position.
 - Arduino Board (serial number: `Arduino1`)
     - The attached sensors are covered in the [Sensors](#attached-sensors) section.
 - Camera (serial number: `Camera`)
@@ -254,6 +255,15 @@ If you see a message saying that Python cannot be found that looks similar to th
 As well as the guidance above, there are a few other points to note when using the simulator.
 These can help you to understand what is happening and how to get the most out of the simulator.
 
+### Using Other Zones
+
+If the arena has multiple starting zones, you can run multiple robots in the simulator.
+To test how your robot behaves in each starting zone of the arena, you can copy your robot's code to run in each corner.
+
+In the folder where you extracted the simulator, alongside the `zone_0` folder, you may have other `zone_<number>` folders.
+Such as `zone_1`, `zone_2`, etc.
+Each of these folders can contain a `robot.py` file that will be run in the corresponding starting zone of the arena.
+
 ### Performance Optimisations
 
 The default settings work for most users however if you are using a less powerful computer or one without a dedicated graphics card (as is the case on many laptops), you may wish to adjust the graphics settings to enable the simulation to run faster.

example_robots/keyboard_robot.py

@@ -1,10 +1,9 @@
 import math
 
 from controller import Keyboard
-from sr.robot3 import A0, A1, A2, Robot
+from sr.robot3 import A0, A1, A2, OUT_H0, Colour, Robot
 
-# Any keys still pressed in the following period will be handled again
-# leading to rprinting sensors multiple times
+# Keyboard sampling period in milliseconds
 KEYBOARD_SAMPLING_PERIOD = 100
 NO_KEY_PRESSED = -1
 
@@ -15,7 +14,11 @@
     "right": (ord("D"), ord("L")),
     "sense": (ord("Q"), ord("U")),
     "see": (ord("E"), ord("O")),
-    "led": (ord("R"), ord("P")),
+    "led": (ord("Z"), ord("M")),
+    "sucker_enable": (ord("X"), ord(",")),
+    "sucker_disable": (ord("C"), ord(".")),
+    "lift_up": (ord("R"), ord("P")),
+    "lift_down": (ord("F"), ord(";")),
     "boost": (Keyboard.SHIFT, Keyboard.CONTROL),
     "angle_unit": (ord("B"), ord("B")),
 }
@@ -88,12 +91,12 @@ def print_sensors(robot: Robot) -> None:
 
     print("Touch sensor readings:")
     for pin, name in touch_sensor_names.items():
-        touching = robot.arduino.pins[pin].digital_value
+        touching = robot.arduino.pins[pin].digital_read()
         print(f"{pin} {name: <6}: {touching}")
 
     print("Reflectance sensor readings:")
     for Apin, name in reflectance_sensor_names.items():
-        reflectance = robot.arduino.pins[Apin].analog_value
+        reflectance = robot.arduino.pins[Apin].analog_read()
         print(f"{Apin} {name: <12}: {reflectance:.2f} V")
 
 
@@ -104,8 +107,9 @@ def print_camera_detection(robot: Robot) -> None:
         for marker in markers:
             print(f" #{marker.id}")
             print(
-                f" Position: {marker.distance:.0f} mm, azi: {angle_str(marker.azimuth)}, "
-                f"elev: {angle_str(marker.elevation)}",
+                f" Position: {marker.position.distance:.0f} mm, "
+                f"{angle_str(marker.position.horizontal_angle)} right, "
+                f"{angle_str(marker.position.vertical_angle)} up",
             )
             yaw, pitch, roll = marker.orientation
             print(
@@ -120,10 +124,16 @@ def print_camera_detection(robot: Robot) -> None:
 
 
 robot = Robot()
-
 keyboard = KeyboardInterface()
+lift_height = robot.servo_board.servos[0].position
+
+# Automatically set the zone controls based on the robot's zone
+# Alternatively, you can set this manually
+# ZONE_CONTROLS = 0
+ZONE_CONTROLS = robot.zone
 
-key_sense = CONTROLS["sense"][robot.zone]
+assert ZONE_CONTROLS < len(CONTROLS["forward"]), \
+    "No controls defined for this zone, alter the ZONE_CONTROLS variable to use in this zone."
 
 print(
     "Note: you need to click on 3D viewport for keyboard events to be picked "
@@ -138,36 +148,58 @@ def print_camera_detection(robot: Robot) -> None:
     keys = keyboard.process_keys()
 
     # Actions that are run continuously while the key is held
-    if CONTROLS["forward"][robot.zone] in keys["held"]:
+    if CONTROLS["forward"][ZONE_CONTROLS] in keys["held"]:
         left_power += 0.5
         right_power += 0.5
 
-    if CONTROLS["reverse"][robot.zone] in keys["held"]:
+    if CONTROLS["reverse"][ZONE_CONTROLS] in keys["held"]:
         left_power += -0.5
         right_power += -0.5
 
-    if CONTROLS["left"][robot.zone] in keys["held"]:
+    if CONTROLS["left"][ZONE_CONTROLS] in keys["held"]:
         left_power -= 0.25
         right_power += 0.25
 
-    if CONTROLS["right"][robot.zone] in keys["held"]:
+    if CONTROLS["right"][ZONE_CONTROLS] in keys["held"]:
         left_power += 0.25
         right_power -= 0.25
 
-    if CONTROLS["boost"][robot.zone] in keys["held"]:
+    if CONTROLS["boost"][ZONE_CONTROLS] in keys["held"]:
         boost = True
 
+    if CONTROLS["lift_up"][ZONE_CONTROLS] in keys["held"]:
+        # constrain to [-1, 1]
+        lift_height = max(min(lift_height + 0.05, 1), -1)
+        robot.servo_board.servos[0].position = lift_height
+
+    if CONTROLS["lift_down"][ZONE_CONTROLS] in keys["held"]:
+        # constrain to [-1, 1]
+        lift_height = max(min(lift_height - 0.05, 1), -1)
+        robot.servo_board.servos[0].position = lift_height
+
     # Actions that are run once when the key is pressed
-    if CONTROLS["sense"][robot.zone] in keys["pressed"]:
+    if CONTROLS["sense"][ZONE_CONTROLS] in keys["pressed"]:
         print_sensors(robot)
 
-    if CONTROLS["see"][robot.zone] in keys["pressed"]:
+    if CONTROLS["see"][ZONE_CONTROLS] in keys["pressed"]:
         print_camera_detection(robot)
 
-    if CONTROLS["led"][robot.zone] in keys["pressed"]:
-        pass
+    if CONTROLS["sucker_enable"][ZONE_CONTROLS] in keys["pressed"]:
+        robot.power_board.outputs[OUT_H0].is_enabled = 1
+
+    if CONTROLS["sucker_disable"][ZONE_CONTROLS] in keys["pressed"]:
+        robot.power_board.outputs[OUT_H0].is_enabled = 0
+
+    if CONTROLS["led"][ZONE_CONTROLS] in keys["pressed"]:
+        robot.kch.leds[0].colour = Colour.MAGENTA
+        robot.kch.leds[1].colour = Colour.MAGENTA
+        robot.kch.leds[2].colour = Colour.MAGENTA
+    elif CONTROLS["led"][ZONE_CONTROLS] in keys["released"]:
+        robot.kch.leds[0].colour = Colour.OFF
+        robot.kch.leds[1].colour = Colour.OFF
+        robot.kch.leds[2].colour = Colour.OFF
 
-    if CONTROLS["angle_unit"][robot.zone] in keys["pressed"]:
+    if CONTROLS["angle_unit"][ZONE_CONTROLS] in keys["pressed"]:
         USE_DEGREES = not USE_DEGREES
         print(f"Angle unit set to {'degrees' if USE_DEGREES else 'radians'}")
 

simulator/controllers/usercode_runner/usercode_runner.py

@@ -48,7 +48,7 @@ def get_robot_file(robot_zone: int) -> Path:
 
     # Check if the robot file exists
     if not robot_file.exists():
-        raise FileNotFoundError(f"No robot controller found for zone {robot_file}")
+        raise FileNotFoundError(f"No robot code to run for zone {robot_zone}")
 
     return robot_file
 
@@ -169,8 +169,9 @@ def main() -> bool:
         robot_file = get_robot_file(zone)
     except FileNotFoundError as e:
         print(e.args[0])
+        robot.step()
         # Not having a robot file is not an error in dev mode
-        return game_mode == 'comp'
+        return game_mode != 'comp'
 
     # Setup log file
     prefix_and_tee_streams(

simulator/environment.py

@@ -11,7 +11,7 @@
 MODULES_ROOT = SIM_ROOT / 'modules'
 GAME_MODE_FILE = SIM_ROOT / 'mode.txt'
 
-NUM_ZONES = 1
+NUM_ZONES = 4
 
 
 def setup_environment() -> None:

simulator/modules/sbot_interface/devices/motor.py

@@ -97,7 +97,7 @@ def __init__(self, device_name: str) -> None:
         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)
+        self._device.setAvailableTorque(2)
 
     def disable(self) -> None:
         """Disable the motor."""

simulator/modules/sbot_interface/devices/power.py

@@ -4,7 +4,7 @@
 from abc import ABC, abstractmethod
 from typing import Callable
 
-from sbot_interface.devices.util import get_globals
+from sbot_interface.devices.util import WebotsDevice, get_globals, get_robot_device
 
 
 class Output:
@@ -36,6 +36,36 @@ def get_current(self) -> int:
         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."""
 

simulator/modules/sbot_interface/devices/servo.py

@@ -4,7 +4,10 @@
 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,
@@ -14,6 +17,9 @@
     map_to_range,
 )
 
+if TYPE_CHECKING:
+    from controller import PositionSensor
+
 MAX_POSITION = 2000
 MIN_POSITION = 1000
 
@@ -97,8 +103,11 @@ def __init__(self, device_name: str) -> None:
         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."""
@@ -112,7 +121,7 @@ def set_position(self, value: int) -> None:
         """
         # Apply a small amount of variation to the power setting to simulate
         # inaccuracies in the servo
-        value = int(add_jitter(value, (MIN_POSITION, MAX_POSITION)))
+        value = int(add_jitter(value, (MIN_POSITION, MAX_POSITION), std_dev_percent=0.5))
 
         self._device.setPosition(map_to_range(
             value,
@@ -128,6 +137,12 @@ def get_position(self) -> int:
 
         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:

simulator/modules/sbot_interface/devices/util.py

@@ -13,6 +13,7 @@
     Accelerometer,
     Camera,
     Compass,
+    Connector,
     DistanceSensor,
     Emitter,
     Gyro,
@@ -45,6 +46,7 @@ class WebotsDevice:
     Accelerometer = Accelerometer
     Camera = Camera
     Compass = Compass
+    Connector = Connector
     DistanceSensor = DistanceSensor
     Emitter = Emitter
     GPS = GPS
@@ -152,6 +154,6 @@ def add_jitter(
     std_dev = value * (std_dev_percent / 100.0)
     mean_offset = value * (offset_percent / 100.0)
 
-    error = value + gauss(mean_offset, std_dev)
+    error = gauss(mean_offset, std_dev)
     # Ensure the error is within the range
     return max(value_range[0], min(value_range[1], value + error))

simulator/modules/sbot_interface/setup.py

@@ -26,8 +26,8 @@
 from sbot_interface.devices.camera import Camera
 from sbot_interface.devices.led import Led, NullLed
 from sbot_interface.devices.motor import Motor
-from sbot_interface.devices.power import NullBuzzer, Output, StartButton
-from sbot_interface.devices.servo import NullServo
+from sbot_interface.devices.power import ConnectorOutput, NullBuzzer, Output, StartButton
+from sbot_interface.devices.servo import NullServo, Servo
 from sbot_interface.socket_server import Board, DeviceServer, SocketServer
 
 
@@ -46,7 +46,10 @@ def setup_devices(log_level: int | str = logging.WARNING) -> SocketServer:
     # this is the configuration of devices connected to the robot
     devices: list[Board] = [
         PowerBoard(
-            outputs=[Output() for _ in range(7)],
+            outputs=(
+                [ConnectorOutput('vacuum sucker')]
+                + [Output() for _ in range(6)]
+            ),
             buzzer=NullBuzzer(),
             button=StartButton(),
             leds=(NullLed(), NullLed()),
@@ -60,7 +63,10 @@ def setup_devices(log_level: int | str = logging.WARNING) -> SocketServer:
             asset_tag='MOT',
         ),
         ServoBoard(
-            servos=[NullServo() for _ in range(8)],
+            servos=(
+                [Servo('vacuum sucker motor::main')]
+                + [NullServo() for _ in range(7)]
+            ),
             asset_tag='SERVO',
         ),
         LedBoard(