Report MPC horizon and drop any cycles at the beginning which are 0
This lets us start firing faster.
Change-Id: Idda068e3b0ca78da6ec5ab422d217d10d9c65e1c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index 6a2c834..12c0116 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -159,6 +159,15 @@
// Returns the time in seconds it last took Solve to run.
double solve_time() const { return solve_time_; }
+ // Returns the horizon of the current controller.
+ size_t current_horizon() const {
+ if (current_controller_ >= problems_.size()) {
+ return 0u;
+ } else {
+ return problems_[current_controller_]->horizon();
+ }
+ }
+
// Returns the controller value if there is a controller to run, or nullopt if
// we finished the last controller. Advances the controller pointer to the
// next controller and warms up the next controller.
@@ -182,7 +191,7 @@
class Catapult {
public:
Catapult(const constants::Values &values)
- : catapult_(values.catapult.subsystem_params), catapult_mpc_(35) {}
+ : catapult_(values.catapult.subsystem_params), catapult_mpc_(30) {}
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
@@ -198,6 +207,8 @@
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.
@@ -230,6 +241,7 @@
bool use_profile_ = true;
int shot_count_ = 0;
+ uint8_t current_horizon_ = 0u;
};
} // namespace catapult