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.h b/y2022/constants.h
index 160da1e..92ae81e 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/climber_plant.h"
#include "y2022/control_loops/superstructure/intake/intake_plant.h"
#include "y2022/control_loops/superstructure/turret/turret_plant.h"
@@ -135,6 +136,31 @@
PotConstants flipper_arm_left;
PotConstants flipper_arm_right;
+
+ // Catapult.
+ static constexpr double kCatapultPotRatio() { return (12.0 / 33.0); }
+ static constexpr double kCatapultEncoderRatio() {
+ return kCatapultPotRatio();
+ }
+ static constexpr double kCatapultEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kMaxCatapultEncoderPulsesPerSecond() {
+ return control_loops::superstructure::catapult::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::catapult::kOutputRatio /
+ kCatapultEncoderRatio() * kCatapultEncoderCountsPerRevolution();
+ }
+ static constexpr ::frc971::constants::Range kCatapultRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -1.2,
+ .upper_hard = 2.0,
+ .lower = -1.00,
+ .upper = 1.57,
+ };
+ }
+
+ PotAndAbsEncoderConstants catapult;
};
// Creates and returns a Values instance for the constants.