From d7a02f0de7d35b4dd58ae1e8e18ef8f98f4527e5 Mon Sep 17 00:00:00 2001
From: James Ward <james@thedropbears.org.au>
Date: Mon, 11 Mar 2024 17:51:38 +1100
Subject: [PATCH] Set starting poses for auto routines, and flash cyan if
 missing

---
 autonomous/autonomous.py | 21 ++++++++++++++++++---
 components/led.py        |  9 +++++++--
 robot.py                 |  2 ++
 3 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py
index b8b2f211..75f5e2f9 100644
--- a/autonomous/autonomous.py
+++ b/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):
diff --git a/components/led.py b/components/led.py
index 4c4d300a..cdc535a2 100644
--- a/components/led.py
+++ b/components/led.py
@@ -95,6 +95,9 @@ def rainbow(self) -> None:
     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)
 
@@ -111,7 +114,8 @@ def execute(self) -> None:
 
 
 class Pattern(Protocol):
-    def update(self) -> Hsv: ...
+    def update(self) -> Hsv:
+        ...
 
 
 @dataclasses.dataclass
@@ -128,7 +132,8 @@ class TimeBasedPattern(ABC, Pattern):
     clock: Callable[[], float] = time.monotonic
 
     @abstractmethod
-    def update(self) -> Hsv: ...
+    def update(self) -> Hsv:
+        ...
 
 
 @dataclasses.dataclass
diff --git a/robot.py b/robot.py
index 67ec446c..7fe23881 100644
--- a/robot.py
+++ b/robot.py
@@ -211,6 +211,8 @@ def disabledPeriodic(self) -> None:
                         self.status_lights.rainbow()
                     else:
                         self.status_lights.invalid_start()
+                else:
+                    self.status_lights.missing_start_pose()
 
     def autonomousInit(self) -> None:
         pass