Driver setup aids

autonomous/autonomous.py

@@ -36,7 +36,12 @@ def __init__(self) -> None:
             Path([ShootingPositions.amp_speaker_bounce]),
             Path([NotePositions.amp]),
         ]
-        super().__init__(note_paths, shoot_paths)
+        # Start pose only needs to be on the correct half of the field,
+        # so choose the podium as a reference point
+        start_pose = Pose2d(
+            NotePositions.podium, rotation_to_red_speaker(NotePositions.podium)
+        )
+        super().__init__(note_paths, shoot_paths, start_pose)
 
 
 class AmpCentre1(AutoBase):
@@ -52,7 +57,12 @@ def __init__(self) -> None:
             Path([NotePositions.amp]),
             Path([PathPositions.avoid_wall, NotePositions.amp]),
         ]
-        super().__init__(note_paths, shoot_paths)
+        # Start pose only needs to be on the correct half of the field,
+        # so choose the amp as a reference point
+        start_pose = Pose2d(
+            NotePositions.amp, rotation_to_red_speaker(NotePositions.amp)
+        )
+        super().__init__(note_paths, shoot_paths, start_pose)
 
 
 class SpeakerCentre3(AutoBase):
@@ -68,7 +78,12 @@ def __init__(self) -> None:
             Path([NotePositions.speaker]),
             Path([PathPositions.stage_transition_N, NotePositions.speaker]),
         ]
-        super().__init__(note_paths, shoot_paths)
+        # Start pose only needs to be on the correct half of the field,
+        # so choose the speaker as a reference point
+        start_pose = Pose2d(
+            NotePositions.speaker, rotation_to_red_speaker(NotePositions.speaker)
+        )
+        super().__init__(note_paths, shoot_paths, start_pose)
 
 
 class Centre3Centre5(AutoBase):

components/led.py

@@ -92,6 +92,15 @@ def morse(self) -> None:
     def rainbow(self) -> None:
         self.pattern = Rainbow(HsvColour.RED)
 
+    def invalid_start(self) -> None:
+        self.pattern = Flash(HsvColour.RED)
+
+    def missing_start_pose(self) -> None:
+        self.pattern = Flash(HsvColour.CYAN)
+
+    def no_vision(self) -> None:
+        self.pattern = Flash(HsvColour.ORANGE)
+
     def disabled(self) -> None:
         self.pattern = Solid(HsvColour.OFF)
 

components/vision.py

@@ -1,4 +1,5 @@
 import math
+import time
 from typing import Optional
 
 import wpilib
@@ -21,6 +22,9 @@ class VisualLocalizer:
     # Give bias to the best pose by multiplying this const to the alt dist
     BEST_POSE_BIAS = 1.2
 
+    # Time since the last target sighting we allow before informing drivers
+    TIMEOUT = 1.0  # s
+
     add_to_estimator = tunable(True)
     should_log = tunable(False)
 
@@ -41,6 +45,7 @@ def __init__(
         self.camera = PhotonCamera(name)
         self.camera_to_robot = Transform3d(pos, rot).inverse()
         self.last_timestamp = -1
+        self.last_recieved_timestep = -1.0
 
         self.single_best_log = field.getObject(name + "single_best_log")
         self.single_alt_log = field.getObject(name + "single_alt_log")
@@ -68,6 +73,7 @@ def execute(self) -> None:
 
         if timestamp == self.last_timestamp:
             return
+        self.last_recieved_timestep = time.monotonic()
         self.last_timestamp = timestamp
 
         if results.multiTagResult.estimatedPose.isPresent:
@@ -140,6 +146,9 @@ def execute(self) -> None:
                         )
                     )
 
+    def sees_target(self):
+        return time.monotonic() - self.last_recieved_timestep < self.TIMEOUT
+
 
 def estimate_poses_from_apriltag(
     cam_to_robot: Transform3d, target: PhotonTrackedTarget

robot.py

@@ -17,11 +17,12 @@
 from controllers.intake import Intake
 from controllers.shooter import Shooter
 
-from utilities.game import is_red
-
+from autonomous.base import AutoBase
 
+from utilities.game import is_red
 from utilities.scalers import rescale_js
 from utilities.functions import clamp
+from utilities.position import on_same_side_of_stage
 
 
 class MyRobot(magicbot.MagicRobot):
@@ -192,6 +193,29 @@ def disabledPeriodic(self) -> None:
         self.vision_port.execute()
         self.vision_starboard.execute()
 
+        # check if we can see targets
+        if (
+            not self.vision_port.sees_target()
+            and not self.vision_starboard.sees_target()
+        ):
+            self.status_lights.no_vision()
+        else:
+            # check we start on the correct side of the stage
+            selected_auto = self._automodes.chooser.getSelected()
+            if isinstance(selected_auto, AutoBase):
+                intended_start_pose = selected_auto.get_starting_pose()
+                if intended_start_pose is not None:
+                    if on_same_side_of_stage(
+                        intended_start_pose, self.chassis.get_pose()
+                    ):
+                        self.status_lights.rainbow()
+                    else:
+                        self.status_lights.invalid_start()
+                else:
+                    self.status_lights.missing_start_pose()
+            else:
+                self.status_lights.missing_start_pose()
+
     def autonomousInit(self) -> None:
         pass
 

utilities/position.py

@@ -56,6 +56,12 @@ class TeamPoses:
     RED_PODIUM = field_flip_pose2d(BLUE_PODIUM)
 
 
+def on_same_side_of_stage(pose1: Pose2d, pose2: Pose2d) -> bool:
+    return not (
+        (pose1.y > TeamPoses.BLUE_PODIUM.y) ^ (pose2.y > TeamPoses.BLUE_PODIUM.y)
+    )
+
+
 class PathPositions:
     stage_transition_N = Translation2d(11.4, 4.5)
     stage_transition_S = Translation2d(11.4, 3.74)