commit 5da0635d23e44c89259297a108c440164dcc8b65
author Ravago Jones <ravagojones@gmail.com> Fri Mar 04 20:26:24 2022 -0800
committer Ravago Jones <ravagojones@gmail.com> Fri Mar 04 20:26:24 2022 -0800
tree 463036ec0a0a0b1f1a329884560424bf5de17190
parent 20b9638eb25b2cbf077d31ef3f27c0e435052e5e

Add superstructure state machine

Managing balls from the intake into the catapult

Change-Id: I88535ee82a876d63fe49e3607c6986690d704daf
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
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 e4d76d3..adc13c7 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -23,7 +23,7 @@
 
   instance.constraint_matrix =
       Eigen::SparseMatrix<double, Eigen::ColMajor, osqp::c_int>(horizon,
-                                                                 horizon);
+                                                                horizon);
   instance.constraint_matrix.setIdentity();
 
   instance.lower_bounds =
@@ -313,7 +313,7 @@
 const flatbuffers::Offset<
     frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
 Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
-                  double *catapult_voltage,
+                  double *catapult_voltage, bool fire,
                   flatbuffers::FlatBufferBuilder *fbb) {
   const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
       *catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
@@ -326,13 +326,12 @@
   if (catapult_disabled) {
     catapult_state_ = CatapultState::PROFILE;
   } else if (catapult_.running() && unsafe_goal &&
-             unsafe_goal->has_catapult() && unsafe_goal->catapult()->fire() &&
-             !last_firing_) {
+             unsafe_goal->has_catapult() && fire && !last_firing_) {
     catapult_state_ = CatapultState::FIRING;
   }
 
   if (catapult_.running() && unsafe_goal && unsafe_goal->has_catapult()) {
-    last_firing_ = unsafe_goal->catapult()->fire();
+    last_firing_ = fire;
   }
 
   use_profile_ = true;
@@ -379,8 +378,7 @@
           use_profile_ = false;
         }
       } else {
-        if (unsafe_goal && unsafe_goal->has_catapult() &&
-            !unsafe_goal->catapult()->fire()) {
+        if (unsafe_goal && unsafe_goal->has_catapult() && !fire) {
           // Eh, didn't manage to solve before it was time to fire.  Give up.
           catapult_state_ = CatapultState::PROFILE;
         }
@@ -417,7 +415,8 @@
     }
   }
 
-  catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage : 0.0);
+  catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage
+                                                       : 0.0);
 
   return catapult_.MakeStatus(fbb);
 }