Merge pull request #190 from thedropbears/stage-tolerance

Check we don't start too close to the stage

components/led.py

@@ -101,6 +101,9 @@ def missing_start_pose(self) -> None:
     def no_vision(self) -> None:
         self.pattern = Flash(HsvColour.ORANGE)
 
+    def too_close_to_stage(self) -> None:
+        self.pattern = Flash(HsvColour.MAGENTA)
+
     def disabled(self) -> None:
         self.pattern = Solid(HsvColour.OFF)
 

robot.py

@@ -22,7 +22,7 @@
 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
+from utilities.position import on_same_side_of_stage, y_close_to_stage
 
 
 class MyRobot(magicbot.MagicRobot):
@@ -204,11 +204,13 @@ def disabledPeriodic(self) -> None:
             selected_auto = self._automodes.chooser.getSelected()
             if isinstance(selected_auto, AutoBase):
                 intended_start_pose = selected_auto.get_starting_pose()
+                current_pose = self.chassis.get_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()
+                    if on_same_side_of_stage(intended_start_pose, current_pose):
+                        if y_close_to_stage(current_pose):
+                            self.status_lights.too_close_to_stage()
+                        else:
+                            self.status_lights.rainbow()
                     else:
                         self.status_lights.invalid_start()
                 else:

utilities/position.py

@@ -76,12 +76,17 @@ class TeamPoses:
     RED_PODIUM = field_flip_pose2d(BLUE_PODIUM)
 
 
-def on_same_side_of_stage(pose1: Pose2d, pose2: Pose2d) -> bool:
+def on_same_side_of_stage(intended_start_pose: Pose2d, current_pose: Pose2d) -> bool:
     return not (
-        (pose1.y > TeamPoses.BLUE_PODIUM.y) ^ (pose2.y > TeamPoses.BLUE_PODIUM.y)
+        (intended_start_pose.y > TeamPoses.BLUE_PODIUM.y)
+        ^ (current_pose.y > TeamPoses.BLUE_PODIUM.y)
     )
 
 
+def y_close_to_stage(pose: Pose2d) -> bool:
+    return abs(pose.y - TeamPoses.BLUE_PODIUM.y) < 0.9
+
+
 class PathPositions:
     stage_transition_N = Translation2d(11.4, 4.5)
     stage_transition_S = Translation2d(11.4, 3.74)