Add catapult MPC and controller class
We want to shoot with the MPC, but we want to decelerate and reset with
a more traditional controller. So, transition to the MPC, and back to a
profiled subsystem.
This makes some tweaks to the solver to get it to converge more
reliably. There's apparently a scale factor which was scaling down the
cost matrices based on the initial p matrix, causing it to not solve
reliably... Good fun.
Change-Id: I721eeaf0b214f8f03cad3112acbef1477671e533
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/constants.cc b/y2022/constants.cc
index 764e82b..1ec0349 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -11,6 +11,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "glog/logging.h"
+#include "y2022/control_loops/superstructure/catapult/integral_catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/integral_climber_plant.h"
#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2022/control_loops/superstructure/turret/integral_turret_plant.h"
@@ -108,6 +109,27 @@
// No integral loops for flipper arms
+ // Catapult
+ Values::PotAndAbsEncoderConstants *const catapult = &r.catapult;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ *const catapult_params = &catapult->subsystem_params;
+
+ catapult_params->zeroing_voltage = 4.0;
+ catapult_params->operating_voltage = 12.0;
+ catapult_params->zeroing_profile_params = {0.5, 2.0};
+ catapult_params->default_profile_params = {15.0, 40.0};
+ catapult_params->range = Values::kCatapultRange();
+ catapult_params->make_integral_loop =
+ &control_loops::superstructure::catapult::MakeIntegralCatapultLoop;
+ catapult_params->zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ catapult_params->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kCatapultEncoderRatio();
+ catapult_params->zeroing_constants.zeroing_threshold = 0.0005;
+ catapult_params->zeroing_constants.moving_buffer_size = 20;
+ catapult_params->zeroing_constants.allowable_encoder_error = 0.9;
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -123,6 +145,9 @@
0.0;
flipper_arm_left->potentiometer_offset = 0.0;
flipper_arm_right->potentiometer_offset = 0.0;
+
+ catapult_params->zeroing_constants.measured_absolute_position = 0.0;
+ catapult->potentiometer_offset = 0.0;
break;
case kCompTeamNumber:
@@ -138,6 +163,9 @@
0.0;
flipper_arm_left->potentiometer_offset = 0.0;
flipper_arm_right->potentiometer_offset = 0.0;
+
+ catapult_params->zeroing_constants.measured_absolute_position = 0.0;
+ catapult->potentiometer_offset = 0.0;
break;
case kPracticeTeamNumber:
@@ -153,6 +181,9 @@
0.0;
flipper_arm_left->potentiometer_offset = 0.0;
flipper_arm_right->potentiometer_offset = 0.0;
+
+ catapult_params->zeroing_constants.measured_absolute_position = 0.0;
+ catapult->potentiometer_offset = 0.0;
break;
case kCodingRobotTeamNumber:
@@ -168,6 +199,9 @@
0.0;
flipper_arm_left->potentiometer_offset = 0.0;
flipper_arm_right->potentiometer_offset = 0.0;
+
+ catapult_params->zeroing_constants.measured_absolute_position = 0.0;
+ catapult->potentiometer_offset = 0.0;
break;
default: