commit 3283ce070e1ce61942722b5f6c2b43e8ab65f086
author Ravago Jones <ravagojones@gmail.com> Wed Mar 09 19:31:29 2022 -0800
committer Ravago Jones <ravagojones@gmail.com> Sun Mar 13 15:26:03 2022 -0700
tree da8a332d595d045224fddf5f044c217c8baa7c7c
parent 2da80bbdd79a136f91fa7c23cf867ceb5e02e026

Add interpolation table boilerplate

Change-Id: I599821c53366e85a9b11a48f7a47888372553773
Signed-off-by: Ravago Jones <ravagojones@gmail.com>

y2022/constants.h

diff --git a/y2022/constants.h b/y2022/constants.h
index ffe1283..21ea7a4 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -8,12 +8,15 @@
 #include "frc971/constants.h"
 #include "frc971/control_loops/pose.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/shooter_interpolation/interpolation.h"
 #include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 #include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
 #include "y2022/control_loops/superstructure/climber/climber_plant.h"
 #include "y2022/control_loops/superstructure/intake/intake_plant.h"
 #include "y2022/control_loops/superstructure/turret/turret_plant.h"
 
+using ::frc971::shooter_interpolation::InterpolationTable;
+
 namespace y2022 {
 namespace constants {
 
@@ -205,6 +208,23 @@
 
   // TODO(milind): set this
   static constexpr double kImuHeight() { return 0.0; }
+
+  struct ShotParams {
+    // Measured in radians
+    double shot_angle;
+    // Muzzle velocity (m/s) of the ball as it is released from the catapult.
+    double shot_velocity;
+
+    static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2) {
+      using ::frc971::shooter_interpolation::Blend;
+      return ShotParams{
+          Blend(coefficient, a1.shot_angle, a2.shot_angle),
+          Blend(coefficient, a1.shot_velocity, a2.shot_velocity),
+      };
+    }
+  };
+
+  InterpolationTable<ShotParams> shot_interpolation_table;
 };
 
 // Creates and returns a Values instance for the constants.