Reset odometry at subwoofer

thedropbears · Mar 16, 2024 · dc612f8 · dc612f8
1 parent 72ac4ad
commit dc612f8
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 15 deletions.
diff --git 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
@@ -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
@@ -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)