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;
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