Add extra note versions of our autonomous routines

autonomous/autonomous.py

@@ -81,6 +81,30 @@ def __init__(self) -> None:
         super().__init__(note_paths, shoot_paths, start_pose)
 
 
+class AmpCentre1Centre2(AutoBase):
+    # The "top" or north notes of the field
+    MODE_NAME = "4 notes: amp, centre 1, centre 2"
+
+    def __init__(self) -> None:
+        note_paths = [
+            Path([NotePositions.amp], face_target=False),
+            Path([PathPositions.avoid_wall, NotePositions.Centre1], face_target=False),
+            Path([PathPositions.avoid_wall, NotePositions.Centre2], face_target=False),
+        ]
+        shoot_paths = [
+            Path([NotePositions.amp], face_target=True),
+            Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
+            Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
+        ]
+        # Start pose only needs to be on the correct half of the field,
+        # so choose the subwoofer
+        start_pose = Pose2d(
+            TeamPoses.RED_TEST_POSE.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+        )
+        super().__init__(note_paths, shoot_paths, start_pose)
+
+
 class SpeakerCentre3(AutoBase):
     # Moving through the stage
     MODE_NAME = "3 notes: speaker, centre 3"
@@ -109,11 +133,79 @@ def __init__(self) -> None:
         super().__init__(note_paths, shoot_paths, start_pose)
 
 
+class SpeakerCentre3Centre4(AutoBase):
+    # Moving through the stage
+    MODE_NAME = "4 notes: speaker, centre 3, centre 4"
+
+    def __init__(self) -> None:
+        note_paths = [
+            Path([NotePositions.speaker], face_target=False),
+            Path(
+                [PathPositions.stage_transition_N, NotePositions.Centre3],
+                face_target=False,
+            ),
+            Path(
+                [PathPositions.stage_transition_N, NotePositions.Centre4],
+                face_target=False,
+            ),
+        ]
+        shoot_paths = [
+            Path([NotePositions.speaker], face_target=True),
+            Path(
+                [PathPositions.stage_transition_N, NotePositions.speaker],
+                face_target=True,
+            ),
+            Path(
+                [PathPositions.stage_transition_N, NotePositions.speaker],
+                face_target=True,
+            ),
+        ]
+        # Start pose only needs to be on the correct half of the field,
+        # so choose the subwoofer
+        start_pose = Pose2d(
+            TeamPoses.RED_TEST_POSE.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+        )
+        super().__init__(note_paths, shoot_paths, start_pose)
+
+
 class Centre3Centre5(AutoBase):
     # Stay in the south of the field to avoid interfering with allies using the close notes
-    MODE_NAME = "3 notes: center 3, center 5"
-    # Not yet tested
-    DISABLED = True
+    MODE_NAME = "3 notes: centre 3, centre 5"
+
+    def __init__(self) -> None:
+        note_paths = [
+            Path(
+                [
+                    PathPositions.stage_transition_S_entry,
+                    PathPositions.stage_transition_S,
+                    NotePositions.Centre3,
+                ],
+                face_target=False,
+            ),
+            Path([NotePositions.Centre5], face_target=False),
+        ]
+
+        shoot_paths = [
+            Path(
+                [
+                    PathPositions.stage_transition_S,
+                    PathPositions.stage_transition_S_entry,
+                    ShootingPositions.source_side,
+                ],
+                face_target=True,
+            ),
+            Path([ShootingPositions.source_side], face_target=True),
+        ]
+        sim_start_pos = Translation2d(15.4, 2.94)
+        rotation = rotation_to_red_speaker(sim_start_pos)
+        sim_start_pose = Pose2d(sim_start_pos, rotation)
+        super().__init__(note_paths, shoot_paths, sim_start_pose)
+
+
+class Centre3Centre4Centre5(AutoBase):
+    # Stay in the south of the field to avoid interfering with allies using the close notes
+    MODE_NAME = "4 notes: centre 3, centre 4, centre 5"
 
     def __init__(self) -> None:
         note_paths = [
@@ -125,6 +217,13 @@ def __init__(self) -> None:
                 ],
                 face_target=False,
             ),
+            Path(
+                [
+                    PathPositions.avoid_stage_S,
+                    NotePositions.Centre4,
+                ],
+                face_target=False,
+            ),
             Path([NotePositions.Centre5], face_target=False),
         ]
 
@@ -137,6 +236,13 @@ def __init__(self) -> None:
                 ],
                 face_target=True,
             ),
+            Path(
+                [
+                    PathPositions.avoid_stage_S,
+                    ShootingPositions.source_side,
+                ],
+                face_target=True,
+            ),
             Path([ShootingPositions.source_side], face_target=True),
         ]
         sim_start_pos = Translation2d(15.4, 2.94)

utilities/position.py

@@ -92,3 +92,4 @@ class PathPositions:
     stage_transition_S = Translation2d(11.4, 3.74)
     stage_transition_S_entry = Translation2d(13.0, 2.5)
     avoid_wall = Translation2d(10.80, 6.55)
+    avoid_stage_S = Translation2d(10.66, 1.40)