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/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index d97befc..68260fd 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -5,6 +5,7 @@
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2022/constants.h"
+#include "y2022/control_loops/superstructure/catapult/catapult.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
@@ -50,6 +51,8 @@
aos::Sender<Status>::Builder *status) override;
private:
+ std::shared_ptr<const constants::Values> values_;
+
RelativeEncoderSubsystem climber_;
PotAndAbsoluteEncoderSubsystem intake_front_;
PotAndAbsoluteEncoderSubsystem intake_back_;
@@ -59,6 +62,8 @@
drivetrain_status_fetcher_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
+
+ catapult::Catapult catapult_;
};
} // namespace superstructure