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.h

diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 4560071..808a175 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -8,9 +8,11 @@
 #include "frc971/control_loops/pose.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
 #include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/averager.h"
 #include "y2024/constants.h"
 #include "y2024/constants/constants_generated.h"
 #include "y2024/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/control_loops/superstructure/superstructure_can_position_static.h"
 #include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2024/control_loops/superstructure/superstructure_status_generated.h"
 using y2024::control_loops::superstructure::AimerStatus;
@@ -19,9 +21,6 @@
 
 class Aimer {
  public:
-  // When the turret is at 0 the note will be leaving the robot at PI.
-  static constexpr double kTurretZeroOffset = 0.11;
-
   Aimer(aos::EventLoop *event_loop, const Constants *robot_constants);
 
   void Update(
@@ -33,6 +32,12 @@
 
   double DistanceToGoal() const { return current_goal_.virtual_shot_distance; }
 
+  // Indicate that we are ready so that we can reset the note current filter to
+  // avoid taking values from when we were still seating the note.
+  void IndicateReady() { note_current_average_.Reset(); }
+
+  void latch_note_current(bool latch) { latch_current_ = latch; }
+
   flatbuffers::Offset<AimerStatus> PopulateStatus(
       flatbuffers::FlatBufferBuilder *fbb) const;
 
@@ -41,6 +46,8 @@
 
   const Constants *robot_constants_;
 
+  bool latch_current_ = false;
+
   frc971::control_loops::drivetrain::DrivetrainConfig<double>
       drivetrain_config_;
 
@@ -52,6 +59,10 @@
       y2024::constants::Values::ShotParams>
       interpolation_table_shuttle_;
 
+  frc971::shooter_interpolation::InterpolationTable<
+      y2024::constants::Values::NoteParams>
+      note_interpolation_table_;
+
   std::map<AutoAimMode, frc971::shooter_interpolation::InterpolationTable<
                             y2024::constants::Values::ShotParams> *>
       interpolation_tables_ = {
@@ -108,6 +119,8 @@
 
   aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
 
+  frc971::zeroing::Averager<double, 50> note_current_average_;
+
   frc971::control_loops::aiming::TurretGoal current_goal_;
 
   bool received_joystick_state_ = false;