commit 73d68883ced7df7a5d669b53b579d1160681a3f1
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Apr 07 21:26:17 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat May 04 20:22:13 2024 -0700
tree b7b81d211e14b61829eff6147076241da090ef0e
parent 548174a01141a1ed962e6ac4fb52868529fc1a04

Adjust shot angle offset depending on retention roller current

This is functionally disabled currently, as it always outputs the same
shot angle offset.

Change-Id: I7bc4b3bc0ba2779d00a3d3203e67e97748b759b1
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

y2024/control_loops/superstructure/aiming.cc

diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index 200029e..a2e92fd 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -22,8 +22,22 @@
           y2024::constants::Values::InterpolationTableFromFlatbuffer(
               robot_constants_->common()
                   ->shooter_shuttle_interpolation_table())),
+      note_interpolation_table_(
+          y2024::constants::Values::NoteInterpolationTableFromFlatbuffer(
+              robot_constants_->common()->note_interpolation_table())),
       joystick_state_fetcher_(
-          event_loop_->MakeFetcher<aos::JoystickState>("/aos")) {}
+          event_loop_->MakeFetcher<aos::JoystickState>("/aos")) {
+  event_loop_->MakeWatcher(
+      "/superstructure/rio",
+      [this](const y2024::control_loops::superstructure::CANPosition &msg) {
+        if (latch_current_) return;
+
+        if (msg.has_retention_roller()) {
+          note_current_average_.AddData(
+              msg.retention_roller()->torque_current());
+        }
+      });
+}
 
 void Aimer::Update(
     const frc971::control_loops::drivetrain::Status *status,
@@ -73,7 +87,14 @@
                      robot_constants_->common()->turret()->range()),
                  current_interpolation_table->Get(current_goal_.target_distance)
                      .shot_speed_over_ground,
-                 /*wrap_mode=*/0.15, M_PI - kTurretZeroOffset},
+                 /*wrap_mode=*/0.15,
+                 // If we don't have any retention roller data, the averager
+                 // will return 0. If we get a current that is out-of-range of
+                 // the interpolation table, we will use the terminal values of
+                 // the interpolation table.
+                 M_PI - note_interpolation_table_
+                            .Get(note_current_average_.GetAverage()(0))
+                            .turret_offset},
       RobotState{
           robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
 
@@ -86,6 +107,9 @@
   builder.add_turret_position(current_goal_.position);
   builder.add_turret_velocity(current_goal_.velocity);
   builder.add_target_distance(current_goal_.target_distance);
-  builder.add_shot_distance(DistanceToGoal());
+  builder.add_note_current(note_current_average_.GetAverage()(0));
+  builder.add_current_turret_offset(
+      note_interpolation_table_.Get(note_current_average_.GetAverage()(0))
+          .turret_offset);
   return builder.Finish();
 }