Fix coordinate scheme

autonomous/autonomous.py

@@ -1,12 +1,19 @@
+import math
+
 from utilities.position import (
     NotePositions,
     Path,
     ShootingPositions,
     PathPositions,
-    rotation_to_red_speaker,
 )
+from utilities import game
 from autonomous.base import AutoBase
-from wpimath.geometry import Pose2d, Translation2d
+from wpimath.geometry import Pose2d, Translation2d, Rotation2d
+
+
+def rotation_to_red_speaker(position: Translation2d) -> Rotation2d:
+    t = game.RED_SPEAKER_POSE.toPose2d().translation() - position
+    return t.angle() + Rotation2d(math.pi)
 
 
 class PodiumSpeakerAmpTopcentre(AutoBase):

autonomous/base.py

@@ -245,6 +245,8 @@ def calculate_trajectory(self, path: Path) -> Trajectory:
             # second last pose might be our our current pose
             second_last = waypoints[-2] if len(waypoints) > 1 else pose.translation()
             disp = endpoint - second_last
+            if not game.is_red():
+                disp = game.field_flip_translation2d(disp)
             heading_target = math.atan2(disp.y, disp.x)
             self.goal_heading = heading_target
 

utilities/game.py

@@ -60,7 +60,7 @@ def field_flip_rotation2d(r: Rotation2d):
 
 
 def field_flip_angle(r: float):
-    return math.atan2(math.sin(r + math.pi), math.cos(r + math.pi))
+    return math.atan2(math.sin(math.pi - r), math.cos(math.pi - r))
 
 
 def field_flip_translation2d(t: Translation2d):

utilities/position.py

@@ -2,12 +2,13 @@
 
 from wpimath.geometry import Rotation2d, Translation2d, Pose2d
 
-from utilities.game import RED_SPEAKER_POSE, field_flip_pose2d, translation_to_goal
-
-
-def rotation_to_red_speaker(position: Translation2d) -> Rotation2d:
-    t = translation_to_goal(position)
-    return t.angle() + Rotation2d(math.pi)
+from utilities.game import (
+    RED_SPEAKER_POSE,
+    field_flip_pose2d,
+    field_flip_translation2d,
+    get_goal_speaker_position,
+    field_flip_angle,
+)
 
 
 class Path:
@@ -19,9 +20,16 @@ def __init__(self, waypoints: list[Translation2d], face_target: bool):
         self.waypoints = waypoints
         self.face_target = face_target
         if face_target:
+            last_waypoint = waypoints[-1]
             self.final_heading = (
-                rotation_to_red_speaker(waypoints[-1]).radians() + math.pi
+                (
+                    get_goal_speaker_position().toTranslation2d()
+                    - field_flip_translation2d(last_waypoint)
+                )
+                .angle()
+                .radians()
             )
+            self.final_heading = field_flip_angle(self.final_heading) + math.pi
         else:
             self.final_heading = 0