Refactor year-agnostic catapult code to frc971
This breaks up the y2022 catapult code into multiple files and moves
them into the frc971 folder. Year-specific parameters are now
provided via the constructors, and the goal message is moved into
frc971 as well.
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I4ea720ae62a7c6c229d6c24a1f08edd7bc5b9728
diff --git a/frc971/control_loops/catapult/catapult.h b/frc971/control_loops/catapult/catapult.h
new file mode 100644
index 0000000..e84ea88
--- /dev/null
+++ b/frc971/control_loops/catapult/catapult.h
@@ -0,0 +1,79 @@
+#ifndef FRC971_CONTROL_LOOPS_CATAPULT_CATAPULT_H_
+#define FRC971_CONTROL_LOOPS_CATAPULT_CATAPULT_H_
+
+#include "frc971/control_loops/catapult/catapult_controller.h"
+#include "frc971/control_loops/catapult/catapult_goal_generated.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace catapult {
+
+// Class to handle transitioning between both the profiled subsystem and the MPC
+// for shooting.
+class Catapult {
+ public:
+ Catapult(frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ catapult_params,
+ StateFeedbackPlant<2, 1, 1> plant)
+ : catapult_(catapult_params), catapult_mpc_(std::move(plant), 30) {}
+
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+ // Resets all state for when WPILib restarts.
+ void Reset() { catapult_.Reset(); }
+
+ void Estop() { catapult_.Estop(); }
+
+ bool zeroed() const { return catapult_.zeroed(); }
+ bool estopped() const { return catapult_.estopped(); }
+ double solve_time() const { return catapult_mpc_.solve_time(); }
+
+ uint8_t mpc_horizon() const { return current_horizon_; }
+
+ bool mpc_active() const { return !use_profile_; }
+
+ // Returns the number of shots taken.
+ int shot_count() const { return shot_count_; }
+
+ // Returns the estimated position
+ double estimated_position() const { return catapult_.estimated_position(); }
+
+ // Runs either the MPC or the profiled subsystem depending on if we are
+ // shooting or not. Returns the status.
+ const flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ Iterate(const CatapultGoal *catapult_goal,
+ const PotAndAbsolutePosition *position, double battery_voltage,
+ double *catapult_voltage, bool fire,
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ private:
+ PotAndAbsoluteEncoderSubsystem catapult_;
+
+ frc971::control_loops::catapult::CatapultController catapult_mpc_;
+
+ enum CatapultState { PROFILE, FIRING, RESETTING };
+
+ 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;
+
+ int shot_count_ = 0;
+ uint8_t current_horizon_ = 0u;
+};
+
+} // namespace catapult
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_CATAPULT_CATAPULT_H_