commit ed8bf812a56bb20dd148ce05b537e6f384067cf7
author Maxwell Henderson <mxwhenderson@gmail.com> Sat Feb 24 16:41:44 2024 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Sat Feb 24 17:38:58 2024 -0800
tree 4e8f95682f112cbdc41987e2c1611bc991c08993
parent f9b35fe79416325c1f1f227cb0905929bce92c0a

Update the offset when aiming

This lets the aimer know 0 for the turret will be pi for the note.

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Icfa8c864232e569ba34e71149cd8ceab5f615442

y2024/control_loops/superstructure/aiming.cc

diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index 980efba..5bb1d19 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -8,6 +8,9 @@
 using frc971::control_loops::aiming::ShotMode;
 using y2024::control_loops::superstructure::Aimer;
 
+// When the turret is at 0 the note will be leaving the robot at PI.
+static constexpr double kTurretZeroOffset = M_PI;
+
 Aimer::Aimer(aos::EventLoop *event_loop,
              const y2024::Constants *robot_constants)
     : event_loop_(event_loop),
@@ -65,7 +68,7 @@
                      robot_constants_->common()->turret()->range()),
                  interpolation_table_.Get(current_goal_.target_distance)
                      .shot_speed_over_ground,
-                 /*wrap_mode=*/0.0, /*turret_zero_offset*/ 0.0},
+                 /*wrap_mode=*/0.0, kTurretZeroOffset},
       RobotState{
           robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
 

y2024/control_loops/superstructure/superstructure_lib_test.cc

diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 701690b..f065850 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -1239,11 +1239,13 @@
 
   EXPECT_NEAR(
       -M_PI_2,
-      superstructure_status_fetcher_->shooter()->aimer()->turret_position(),
+      superstructure_status_fetcher_->shooter()->aimer()->turret_position() -
+          M_PI,
       5e-4);
-  EXPECT_NEAR(-M_PI_2,
-              superstructure_status_fetcher_->shooter()->turret()->position(),
-              5e-4);
+  EXPECT_NEAR(
+      -M_PI_2,
+      superstructure_status_fetcher_->shooter()->turret()->position() - M_PI,
+      5e-4);
 
   EXPECT_EQ(
       kDistanceFromSpeaker,
@@ -1280,11 +1282,13 @@
 
   EXPECT_NEAR(
       M_PI_2,
-      superstructure_status_fetcher_->shooter()->aimer()->turret_position(),
+      superstructure_status_fetcher_->shooter()->aimer()->turret_position() +
+          M_PI,
       5e-4);
-  EXPECT_NEAR(M_PI_2,
-              superstructure_status_fetcher_->shooter()->turret()->position(),
-              5e-4);
+  EXPECT_NEAR(
+      M_PI_2,
+      superstructure_status_fetcher_->shooter()->turret()->position() + M_PI,
+      5e-4);
   EXPECT_EQ(
       kDistanceFromSpeaker,
       superstructure_status_fetcher_->shooter()->aimer()->target_distance());