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
diff --git a/y2022/control_loops/superstructure/catapult_plotter.ts b/y2022/control_loops/superstructure/catapult_plotter.ts
index cfa360d..6c66340 100644
--- a/y2022/control_loops/superstructure/catapult_plotter.ts
+++ b/y2022/control_loops/superstructure/catapult_plotter.ts
@@ -26,7 +26,9 @@
positionPlot.addMessageLine(goal, ['catapult', 'return_position', 'unsafe_goal']).setColor(BLUE).setPointSize(1.0);
positionPlot.addMessageLine(goal, ['catapult', 'fire']).setColor(WHITE).setPointSize(1.0);
- positionPlot.addMessageLine(status, ['mpc_active']).setColor(WHITE).setPointSize(3.0);
+ positionPlot.addMessageLine(status, ['mpc_horizon'])
+ .setColor(WHITE)
+ .setPointSize(3.0);
positionPlot.addMessageLine(status, ['catapult', 'goal_position']).setColor(RED).setPointSize(4.0);
positionPlot.addMessageLine(status, ['catapult', 'goal_velocity']).setColor(ORANGE).setPointSize(4.0);
positionPlot.addMessageLine(status, ['catapult', 'position']).setColor(GREEN).setPointSize(4.0);
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 1168b88..15201f7 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -488,7 +488,7 @@
status_builder.add_catapult(catapult_status_offset);
status_builder.add_solve_time(catapult_.solve_time());
status_builder.add_shot_count(catapult_.shot_count());
- status_builder.add_mpc_active(catapult_.mpc_active());
+ status_builder.add_mpc_horizon(catapult_.mpc_horizon());
if (catapult_goal != nullptr) {
status_builder.add_shot_position(catapult_goal->shot_position());
status_builder.add_shot_velocity(catapult_goal->shot_velocity());
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 389c01a..08edbf0 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -197,7 +197,7 @@
superstructure_status_fetcher_->intake_back());
turret_.Simulate(superstructure_output_fetcher_->turret_voltage(),
superstructure_status_fetcher_->turret());
- if (superstructure_status_fetcher_->mpc_active()) {
+ if (superstructure_status_fetcher_->mpc_horizon()) {
catapult_.set_controller_index(0);
} else {
catapult_.set_controller_index(1);
@@ -1136,7 +1136,7 @@
RunFor(chrono::seconds(5));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_FALSE(superstructure_status_fetcher_->mpc_active());
+ EXPECT_EQ(superstructure_status_fetcher_->mpc_horizon(), 0u);
EXPECT_FLOAT_EQ(superstructure_status_fetcher_->catapult()->position(),
constants::Values::kCatapultRange().lower);
@@ -1188,7 +1188,7 @@
RunFor(chrono::milliseconds(200));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_TRUE(superstructure_status_fetcher_->mpc_active());
+ EXPECT_NE(superstructure_status_fetcher_->mpc_horizon(), 0u);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
EXPECT_GT(superstructure_status_fetcher_->catapult()->position(),
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 92f3dbd..a1817a7 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -72,7 +72,7 @@
catapult:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6);
solve_time:double (id: 7);
- mpc_active:bool (id: 8);
+ mpc_horizon:uint8 (id: 8);
shot_position:double (id: 16);
shot_velocity:double (id: 17);
diff --git a/y2022/www/field.html b/y2022/www/field.html
index e2a1cf6..f39c1a4 100644
--- a/y2022/www/field.html
+++ b/y2022/www/field.html
@@ -53,7 +53,7 @@
</tr>
<tr>
<td>MPC Active</td>
- <td id="mpc_active"> NA </td>
+ <td id="mpc_horizon"> NA </td>
</tr>
<tr>
<td>Shot Count</td>
diff --git a/y2022/www/field_handler.ts b/y2022/www/field_handler.ts
index 584b65e..0c0bf2c 100644
--- a/y2022/www/field_handler.ts
+++ b/y2022/www/field_handler.ts
@@ -49,8 +49,8 @@
(document.getElementById('fire') as HTMLElement);
private mpcSolveTime: HTMLElement =
(document.getElementById('mpc_solve_time') as HTMLElement);
- private mpcActive: HTMLElement =
- (document.getElementById('mpc_active') as HTMLElement);
+ private mpcHorizon: HTMLElement =
+ (document.getElementById('mpc_horizon') as HTMLElement);
private shotCount: HTMLElement =
(document.getElementById('shot_count') as HTMLElement);
private catapult: HTMLElement =
@@ -326,8 +326,8 @@
this.fire.innerHTML = this.superstructureStatus.fire() ? 'true' : 'false';
- this.mpcActive.innerHTML =
- this.superstructureStatus.mpcActive() ? 'true' : 'false';
+ this.mpcHorizon.innerHTML =
+ this.superstructureStatus.mpcHorizon().toFixed(2);
this.setValue(this.mpcSolveTime, this.superstructureStatus.solveTime());