From dc612f82d95adf5e04e6fcbc368ac4e2b7fafc09 Mon Sep 17 00:00:00 2001
From: Michael Kennedy <34204085+mlists@users.noreply.github.com>
Date: Sun, 17 Mar 2024 09:25:47 +1100
Subject: [PATCH] Reset odometry at subwoofer

---
 autonomous/autonomous.py | 16 ++++++++--------
 components/chassis.py    | 10 +++++-----
 utilities/position.py    |  4 ++--
 3 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py
index e8948151..0a86aa25 100644
--- a/autonomous/autonomous.py
+++ b/autonomous/autonomous.py
@@ -34,8 +34,8 @@ def __init__(self) -> None:
             Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True),
         ]
         start_pose = Pose2d(
-            TeamPoses.RED_TEST_POSE.translation(),
-            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+            TeamPoses.RED_SUBWOOFER.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_SUBWOOFER.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
@@ -57,8 +57,8 @@ def __init__(self) -> None:
         # 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()),
+            TeamPoses.RED_SUBWOOFER.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_SUBWOOFER.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
@@ -127,8 +127,8 @@ def __init__(self) -> None:
             ),
         ]
         start_pose = Pose2d(
-            TeamPoses.RED_TEST_POSE.translation(),
-            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+            TeamPoses.RED_SUBWOOFER.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_SUBWOOFER.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
@@ -161,8 +161,8 @@ def __init__(self) -> None:
             ),
         ]
         start_pose = Pose2d(
-            TeamPoses.RED_TEST_POSE.translation(),
-            rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()),
+            TeamPoses.RED_SUBWOOFER.translation(),
+            rotation_to_red_speaker(TeamPoses.RED_SUBWOOFER.translation()),
         )
         super().__init__(note_paths, shoot_paths, start_pose)
 
diff --git a/components/chassis.py b/components/chassis.py
index f4ba0569..3b1cfe3a 100644
--- a/components/chassis.py
+++ b/components/chassis.py
@@ -311,7 +311,7 @@ def get_module_states(
         )
 
     def setup(self) -> None:
-        initial_pose = TeamPoses.RED_TEST_POSE if is_red() else TeamPoses.BLUE_TEST_POSE
+        initial_pose = TeamPoses.RED_SUBWOOFER if is_red() else TeamPoses.BLUE_SUBWOOFER
 
         self.estimator = SwerveDrive4PoseEstimator(
             self.kinematics,
@@ -419,9 +419,9 @@ def update_alliance(self) -> None:
         if is_red() != self.on_red_alliance:
             self.on_red_alliance = is_red()
             if self.on_red_alliance:
-                self.set_pose(TeamPoses.RED_TEST_POSE)
+                self.set_pose(TeamPoses.RED_SUBWOOFER)
             else:
-                self.set_pose(TeamPoses.BLUE_TEST_POSE)
+                self.set_pose(TeamPoses.BLUE_SUBWOOFER)
 
     def update_odometry(self) -> None:
         self.estimator.update(self.imu.getRotation2d(), self.get_module_positions())
@@ -451,9 +451,9 @@ def reset_yaw(self) -> None:
     def reset_odometry(self) -> None:
         """Reset odometry to current team's podium"""
         if is_red():
-            self.set_pose(TeamPoses.RED_PODIUM)
+            self.set_pose(TeamPoses.RED_SUBWOOFER)
         else:
-            self.set_pose(TeamPoses.BLUE_PODIUM)
+            self.set_pose(TeamPoses.BLUE_SUBWOOFER)
 
     def get_module_positions(
         self,
diff --git a/utilities/position.py b/utilities/position.py
index 6ebef9db..798ddb0b 100644
--- a/utilities/position.py
+++ b/utilities/position.py
@@ -70,8 +70,8 @@ class ShootingPositions:
 
 
 class TeamPoses:
-    RED_TEST_POSE = Pose2d(15.1, 5.5, math.pi)
-    BLUE_TEST_POSE = field_flip_pose2d(RED_TEST_POSE)
+    RED_SUBWOOFER = Pose2d(15.175, 5.5, math.pi)
+    BLUE_SUBWOOFER = field_flip_pose2d(RED_SUBWOOFER)
     BLUE_PODIUM = Pose2d(Translation2d(2.992, 4.08455), Rotation2d(math.pi))
     RED_PODIUM = field_flip_pose2d(BLUE_PODIUM)
     RED_AMP_START_POSE = Pose2d(15.9, 6.7, math.pi)