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.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index 8b5c7eb..e662f30 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -300,10 +300,19 @@
return std::nullopt;
}
- const double u = problems_[current_controller_]->U(0);
+ double u;
+ size_t solution_horizon = 0;
+ if (current_controller_ == 0u) {
+ while (solution_horizon < problems_[current_controller_]->horizon() &&
+ problems_[current_controller_]->U(solution_horizon) < 0.01) {
+ ++solution_horizon;
+ }
+ } else {
+ u = problems_[current_controller_]->U(0);
+ }
- if (current_controller_ + 1u < problems_.size()) {
- problems_[current_controller_ + 1]->WarmStart(
+ if (current_controller_ + 1u + solution_horizon < problems_.size()) {
+ problems_[current_controller_ + solution_horizon + 1]->WarmStart(
*problems_[current_controller_]);
}
++current_controller_;
@@ -360,8 +369,9 @@
Eigen::Vector2d(latched_shot_position, latched_shot_velocity));
const bool solved = catapult_mpc_.Solve();
-
- if (solved || catapult_mpc_.started()) {
+ current_horizon_ = catapult_mpc_.current_horizon();
+ const bool started = catapult_mpc_.started();
+ if (solved || started) {
std::optional<double> solution = catapult_mpc_.Next();
if (!solution.has_value()) {
@@ -419,6 +429,7 @@
// Select the controller designed for when we have no ball.
catapult_.set_controller_index(1);
+ current_horizon_ = 0u;
const double output_voltage = catapult_.UpdateController(catapult_disabled);
if (catapult_voltage != nullptr) {
*catapult_voltage = output_voltage;