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

diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 97e319d..de4c117 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -1,6 +1,8 @@
 #ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_AIMING_H_
 #define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_AIMING_H_
 
+#include <map>
+
 #include "frc971/control_loops/aiming/aiming.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/pose.h"
@@ -9,8 +11,8 @@
 #include "y2024/constants.h"
 #include "y2024/constants/constants_generated.h"
 #include "y2024/control_loops/drivetrain/drivetrain_base.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;
 
 namespace y2024::control_loops::superstructure {
@@ -26,7 +28,8 @@
       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);
 
   double DistanceToGoal() const { return current_goal_.virtual_shot_distance; }
 
@@ -45,6 +48,64 @@
       y2024::constants::Values::ShotParams>
       interpolation_table_;
 
+  frc971::shooter_interpolation::InterpolationTable<
+      y2024::constants::Values::ShotParams>
+      interpolation_table_shuttle_;
+
+  std::map<AutoAimMode, frc971::shooter_interpolation::InterpolationTable<
+                            y2024::constants::Values::ShotParams> *>
+      interpolation_tables_ = {
+          {AutoAimMode::SPEAKER, &interpolation_table_},
+          {AutoAimMode::SHUTTLE, &interpolation_table_shuttle_}};
+
+  std::map<AutoAimMode, frc971::control_loops::Pose> red_alliance_goals_ = {
+      {AutoAimMode::SPEAKER,
+       frc971::control_loops::Pose(
+           frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+                                           ->shooter_targets()
+                                           ->red_alliance()
+                                           ->pos()),
+           robot_constants_->common()
+               ->shooter_targets()
+               ->red_alliance()
+               ->theta())},
+      {
+          AutoAimMode::SHUTTLE,
+          frc971::control_loops::Pose(
+              frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+                                              ->shooter_shuttle_targets()
+                                              ->red_alliance()
+                                              ->pos()),
+              robot_constants_->common()
+                  ->shooter_shuttle_targets()
+                  ->red_alliance()
+                  ->theta()),
+      }};
+
+  std::map<AutoAimMode, frc971::control_loops::Pose> blue_alliance_goals_ = {
+      {AutoAimMode::SPEAKER,
+       frc971::control_loops::Pose(
+           frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+                                           ->shooter_targets()
+                                           ->blue_alliance()
+                                           ->pos()),
+           robot_constants_->common()
+               ->shooter_targets()
+               ->blue_alliance()
+               ->theta())},
+      {
+          AutoAimMode::SHUTTLE,
+          frc971::control_loops::Pose(
+              frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+                                              ->shooter_shuttle_targets()
+                                              ->blue_alliance()
+                                              ->pos()),
+              robot_constants_->common()
+                  ->shooter_shuttle_targets()
+                  ->blue_alliance()
+                  ->theta()),
+      }};
+
   aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
 
   frc971::control_loops::aiming::TurretGoal current_goal_;