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/control_loops/superstructure/catapult/catapult.cc

diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index 4a644e2..8b5c7eb 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -312,25 +312,28 @@
 
 const flatbuffers::Offset<
     frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
+Catapult::Iterate(const CatapultGoal *catapult_goal, const Position *position,
                   double battery_voltage, double *catapult_voltage, bool fire,
                   flatbuffers::FlatBufferBuilder *fbb) {
   const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
-      *catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
-                           ? (unsafe_goal->catapult()->return_position())
-                           : nullptr;
+      *return_goal =
+          catapult_goal != nullptr && catapult_goal->has_return_position()
+              ? catapult_goal->return_position()
+              : nullptr;
 
   const bool catapult_disabled = catapult_.Correct(
-      catapult_goal, position->catapult(), catapult_voltage == nullptr);
+      return_goal, position->catapult(), catapult_voltage == nullptr);
 
   if (catapult_disabled) {
     catapult_state_ = CatapultState::PROFILE;
-  } else if (catapult_.running() && unsafe_goal &&
-             unsafe_goal->has_catapult() && fire && !last_firing_) {
+  } else if (catapult_.running() && catapult_goal != nullptr && fire &&
+             !last_firing_) {
     catapult_state_ = CatapultState::FIRING;
+    latched_shot_position = catapult_goal->shot_position();
+    latched_shot_velocity = catapult_goal->shot_velocity();
   }
 
-  if (catapult_.running() && unsafe_goal && unsafe_goal->has_catapult()) {
+  if (catapult_.running()) {
     last_firing_ = fire;
   }
 
@@ -354,8 +357,7 @@
 
       catapult_mpc_.SetState(
           next_X.block<2, 1>(0, 0),
-          Eigen::Vector2d(unsafe_goal->catapult()->shot_position(),
-                          unsafe_goal->catapult()->shot_velocity()));
+          Eigen::Vector2d(latched_shot_position, latched_shot_velocity));
 
       const bool solved = catapult_mpc_.Solve();
 
@@ -379,7 +381,7 @@
           use_profile_ = false;
         }
       } else {
-        if (unsafe_goal && unsafe_goal->has_catapult() && !fire) {
+        if (!fire) {
           // Eh, didn't manage to solve before it was time to fire.  Give up.
           catapult_state_ = CatapultState::PROFILE;
         }

y2022/control_loops/superstructure/catapult/catapult.h

diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index 0606c0d..6a2c834 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -210,14 +210,11 @@
   // shooting or not.  Returns the status.
   const flatbuffers::Offset<
       frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-  Iterate(const Goal *unsafe_goal, const Position *position,
+  Iterate(const CatapultGoal *unsafe_goal, const Position *position,
           double battery_voltage, double *catapult_voltage, bool fire,
           flatbuffers::FlatBufferBuilder *fbb);
 
  private:
-  // TODO(austin): Prototype is just an encoder.  Catapult has both an encoder
-  // and pot.  Switch back once we have a catapult.
-  // PotAndAbsoluteEncoderSubsystem catapult_;
   PotAndAbsoluteEncoderSubsystem catapult_;
 
   catapult::CatapultController catapult_mpc_;
@@ -226,6 +223,9 @@
 
   CatapultState catapult_state_ = CatapultState::PROFILE;
 
+  double latched_shot_position = 0.0;
+  double latched_shot_velocity = 0.0;
+
   bool last_firing_ = false;
   bool use_profile_ = true;