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/joystick_reader.cc b/y2022/joystick_reader.cc
index 80747a8..a39c130 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -10,6 +10,7 @@
#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/input/action_joystick_input.h"
#include "frc971/input/driver_station_data.h"
#include "frc971/input/drivetrain_input.h"
@@ -18,6 +19,7 @@
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::input::driver_station::ButtonLocation;
using frc971::input::driver_station::ControlBit;
using frc971::input::driver_station::JoystickAxis;
@@ -27,6 +29,9 @@
namespace input {
namespace joysticks {
+const ButtonLocation kCatapultPos(3, 3);
+const ButtonLocation kFire(3, 1);
+
namespace superstructure = y2022::control_loops::superstructure;
class Reader : public ::frc971::input::ActionJoystickInput {
@@ -45,12 +50,44 @@
void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
void HandleTeleop(
- const ::frc971::input::driver_station::Data & /*data*/) override {
+ const ::frc971::input::driver_station::Data &data) override {
superstructure_status_fetcher_.Fetch();
if (!superstructure_status_fetcher_.get()) {
AOS_LOG(ERROR, "Got no superstructure status message.\n");
return;
}
+
+ aos::Sender<superstructure::Goal>::Builder builder =
+ superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<frc971::ProfileParameters> catapult_profile =
+ frc971::CreateProfileParameters(*builder.fbb(), 5.0, 30.0);
+
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder
+ catapult_return_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+ catapult_return_builder.add_unsafe_goal(
+ data.IsPressed(kCatapultPos) ? 0.3 : -0.85);
+ catapult_return_builder.add_profile_params(catapult_profile);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ catapult_return_offset = catapult_return_builder.Finish();
+
+ superstructure::CatapultGoal::Builder catapult_builder =
+ builder.MakeBuilder<superstructure::CatapultGoal>();
+ catapult_builder.add_return_position(catapult_return_offset);
+ catapult_builder.add_fire(data.IsPressed(kFire));
+ catapult_builder.add_shot_position(0.3);
+ catapult_builder.add_shot_velocity(15.0);
+ flatbuffers::Offset<superstructure::CatapultGoal> catapult_offset =
+ catapult_builder.Finish();
+
+ superstructure::Goal::Builder goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+ goal_builder.add_catapult(catapult_offset);
+
+ if (builder.Send(goal_builder.Finish()) != aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
}
private: