commit 1286c011fbee333b7a6301ce7ead1ce4dc0164e0
author Filip Kujawa <filip.j.kujawa@gmail.com> Sun Mar 31 22:53:27 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Apr 07 13:26:40 2024 -0700
tree cff11dc7c79ee19d5b77a4c274552d6071d1ce84
parent b364ed4648871bf4594c857d2055c2c76e719451

Add shuttle auto-aim

This change prepares the code for shuttle support by adding a new button,
shot table, and shot target location for the shuttle shot.

Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: Ic70f1b0344fcab8316553cbda4a337cf8353dfd6

y2024/control_loops/superstructure/aiming.cc

diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index bf68527..200029e 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -18,6 +18,10 @@
       interpolation_table_(
           y2024::constants::Values::InterpolationTableFromFlatbuffer(
               robot_constants_->common()->shooter_interpolation_table())),
+      interpolation_table_shuttle_(
+          y2024::constants::Values::InterpolationTableFromFlatbuffer(
+              robot_constants_->common()
+                  ->shooter_shuttle_interpolation_table())),
       joystick_state_fetcher_(
           event_loop_->MakeFetcher<aos::JoystickState>("/aos")) {}
 
@@ -25,7 +29,12 @@
     const frc971::control_loops::drivetrain::Status *status,
     frc971::control_loops::aiming::ShotMode shot_mode,
     frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic
-        *turret_goal) {
+        *turret_goal,
+    AutoAimMode auto_aim_mode) {
+  // Default to aiming at the speaker in the absence of any other targets.
+  if (auto_aim_mode == AutoAimMode::NONE) {
+    auto_aim_mode = AutoAimMode::SPEAKER;
+  }
   if (status == nullptr) {
     return;
   }
@@ -41,22 +50,13 @@
     alliance = joystick_state_fetcher_->alliance();
   }
 
-  const frc971::control_loops::Pose red_alliance_goal(
-      frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
-                                      ->shooter_targets()
-                                      ->red_alliance()
-                                      ->pos()),
-      robot_constants_->common()->shooter_targets()->red_alliance()->theta());
-
-  const frc971::control_loops::Pose blue_alliance_goal(
-      frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
-                                      ->shooter_targets()
-                                      ->blue_alliance()
-                                      ->pos()),
-      robot_constants_->common()->shooter_targets()->blue_alliance()->theta());
+  frc971::shooter_interpolation::InterpolationTable<
+      y2024::constants::Values::ShotParams> *current_interpolation_table =
+      interpolation_tables_.at(auto_aim_mode);
 
   const frc971::control_loops::Pose goal =
-      alliance == aos::Alliance::kRed ? red_alliance_goal : blue_alliance_goal;
+      alliance == aos::Alliance::kRed ? red_alliance_goals_.at(auto_aim_mode)
+                                      : blue_alliance_goals_.at(auto_aim_mode);
 
   const Eigen::Vector2d linear_angular =
       drivetrain_config_.Tlr_to_la() *
@@ -71,7 +71,7 @@
       ShotConfig{goal, shot_mode,
                  frc971::constants::Range::FromFlatbuffer(
                      robot_constants_->common()->turret()->range()),
-                 interpolation_table_.Get(current_goal_.target_distance)
+                 current_interpolation_table->Get(current_goal_.target_distance)
                      .shot_speed_over_ground,
                  /*wrap_mode=*/0.15, M_PI - kTurretZeroOffset},
       RobotState{