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/BUILD b/y2022/BUILD
index 1aa4fa1..b4cac85 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -193,6 +193,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2022/control_loops/superstructure/catapult:catapult_plants",
"//y2022/control_loops/superstructure/climber:climber_plants",
"//y2022/control_loops/superstructure/intake:intake_plants",
"//y2022/control_loops/superstructure/turret:turret_plants",
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:
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.
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index e5992ef..f08bb80 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -76,6 +76,7 @@
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2022:constants",
+ "//y2022/control_loops/superstructure/catapult",
],
)
diff --git a/y2022/control_loops/superstructure/catapult/BUILD b/y2022/control_loops/superstructure/catapult/BUILD
index 289dfde..a43925e 100644
--- a/y2022/control_loops/superstructure/catapult/BUILD
+++ b/y2022/control_loops/superstructure/catapult/BUILD
@@ -42,6 +42,10 @@
":catapult_plants",
"//aos:realtime",
"//third_party/osqp-cpp",
+ "//y2022:constants",
+ "//y2022/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2022/control_loops/superstructure:superstructure_position_fbs",
+ "//y2022/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index 243cb2d..00bb3b3 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -19,17 +19,17 @@
osqp::OsqpInstance MakeInstance(
size_t horizon, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P) {
osqp::OsqpInstance instance;
- instance.objective_matrix = P.cast<c_float>().sparseView();
+ instance.objective_matrix = P.sparseView();
instance.constraint_matrix =
- Eigen::SparseMatrix<c_float, Eigen::ColMajor, osqp::c_int>(horizon,
+ Eigen::SparseMatrix<double, Eigen::ColMajor, osqp::c_int>(horizon,
horizon);
instance.constraint_matrix.setIdentity();
instance.lower_bounds =
- Eigen::Matrix<c_float, Eigen::Dynamic, 1>::Zero(horizon, 1);
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(horizon, 1);
instance.upper_bounds =
- Eigen::Matrix<c_float, Eigen::Dynamic, 1>::Ones(horizon, 1) * 12.0;
+ Eigen::Matrix<double, Eigen::Dynamic, 1>::Ones(horizon, 1) * 12.0;
return instance;
}
} // namespace
@@ -40,27 +40,37 @@
Eigen::Matrix<double, 2, 2> Af,
Eigen::Matrix<double, Eigen::Dynamic, 2> final_q)
: horizon_(horizon),
- accel_q_(std::move(accel_q.cast<c_float>())),
- Af_(std::move(Af.cast<c_float>())),
- final_q_(std::move(final_q.cast<c_float>())),
+ accel_q_(std::move(accel_q)),
+ Af_(std::move(Af)),
+ final_q_(std::move(final_q)),
instance_(MakeInstance(horizon, std::move(P))) {
- instance_.objective_vector =
- Eigen::Matrix<c_float, Eigen::Dynamic, 1>(horizon, 1);
+ // Start with a representative problem.
+ Eigen::Matrix<double, 2, 1> X_initial(0.0, 0.0);
+ Eigen::Matrix<double, 2, 1> X_final(2.0, 25.0);
+
+ objective_vector_ =
+ X_initial(1, 0) * accel_q_ + final_q_ * (Af_ * X_initial - X_final);
+ instance_.objective_vector = objective_vector_;
settings_.max_iter = 25;
- settings_.check_termination = 25;
- settings_.warm_start = 0;
+ settings_.check_termination = 5;
+ settings_.warm_start = 1;
+ // TODO(austin): Do we need this scaling thing? It makes it not solve
+ // sometimes... I'm pretty certain by giving it a decently formed problem to
+ // initialize with, it will not try doing crazy things with the scaling
+ // internally.
+ settings_.scaling = 0;
auto status = solver_.Init(instance_, settings_);
CHECK(status.ok()) << status;
}
-void MPCProblem::SetState(Eigen::Matrix<c_float, 2, 1> X_initial,
- Eigen::Matrix<c_float, 2, 1> X_final) {
+void MPCProblem::SetState(Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final) {
X_initial_ = X_initial;
X_final_ = X_final;
- instance_.objective_vector =
+ objective_vector_ =
X_initial(1, 0) * accel_q_ + final_q_ * (Af_ * X_initial - X_final);
- auto status = solver_.SetObjectiveVector(instance_.objective_vector);
+ auto status = solver_.SetObjectiveVector(objective_vector_);
CHECK(status.ok()) << status;
}
@@ -126,8 +136,8 @@
}
const Eigen::Matrix<double, Eigen::Dynamic, 1> CatapultProblemGenerator::q(
- size_t horizon, Eigen::Matrix<c_float, 2, 1> X_initial,
- Eigen::Matrix<c_float, 2, 1> X_final) {
+ size_t horizon, Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final) {
CHECK_GT(horizon, 0u);
CHECK_LE(horizon, horizon_);
return 2.0 * X_initial(1, 0) * accel_q(horizon) +
@@ -254,6 +264,163 @@
Bf(horizon_).transpose() * Q_final_ * Bf(horizon_));
}
+CatapultController::CatapultController(size_t horizon) : generator_(horizon) {
+ problems_.reserve(generator_.horizon());
+ for (size_t i = generator_.horizon(); i > 0; --i) {
+ problems_.emplace_back(generator_.MakeProblem(i));
+ }
+
+ Reset();
+}
+
+void CatapultController::Reset() {
+ current_controller_ = 0;
+ solve_time_ = 0.0;
+}
+
+void CatapultController::SetState(Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final) {
+ if (current_controller_ >= problems_.size()) {
+ return;
+ }
+ problems_[current_controller_]->SetState(X_initial, X_final);
+}
+
+bool CatapultController::Solve() {
+ if (current_controller_ >= problems_.size()) {
+ return true;
+ }
+ const bool result = problems_[current_controller_]->Solve();
+ solve_time_ = problems_[current_controller_]->solve_time();
+ return result;
+}
+
+std::optional<double> CatapultController::Next() {
+ if (current_controller_ >= problems_.size()) {
+ return std::nullopt;
+ }
+
+ const double u = problems_[current_controller_]->U(0);
+
+ if (current_controller_ + 1u < problems_.size()) {
+ problems_[current_controller_ + 1]->WarmStart(
+ *problems_[current_controller_]);
+ }
+ ++current_controller_;
+ return u;
+}
+
+const flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
+ double *catapult_voltage,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
+ ? (unsafe_goal->catapult()->return_position())
+ : nullptr;
+
+ const bool catapult_disabled = catapult_.Correct(
+ catapult_goal, position->catapult(), catapult_voltage == nullptr);
+
+ if (catapult_disabled) {
+ catapult_state_ = CatapultState::PROFILE;
+ } else if (catapult_.running() && unsafe_goal &&
+ unsafe_goal->has_catapult() && unsafe_goal->catapult()->fire() &&
+ !last_firing_) {
+ catapult_state_ = CatapultState::FIRING;
+ }
+
+ if (catapult_.running() && unsafe_goal && unsafe_goal->has_catapult()) {
+ last_firing_ = unsafe_goal->catapult()->fire();
+ }
+
+ use_profile_ = true;
+
+ switch (catapult_state_) {
+ case CatapultState::FIRING: {
+ // Select the ball controller. We should only be firing if we have a
+ // ball, or at least should only care about the shot accuracy.
+ catapult_.set_controller_index(0);
+ // Ok, so we've now corrected. Next step is to run the MPC.
+ //
+ // Since there is a unit delay between when we ask for a U and the
+ // hardware applies it, we need to run the optimizer for the position at
+ // the *next* control loop cycle.
+
+ const Eigen::Vector3d next_X =
+ catapult_.controller().plant().A() * catapult_.estimated_state() +
+ catapult_.controller().plant().B() *
+ catapult_.controller().observer().last_U();
+
+ catapult_mpc_.SetState(
+ next_X.block<2, 1>(0, 0),
+ Eigen::Vector2d(unsafe_goal->catapult()->shot_position(),
+ unsafe_goal->catapult()->shot_velocity()));
+
+ const bool solved = catapult_mpc_.Solve();
+
+ if (solved || catapult_mpc_.started()) {
+ std::optional<double> solution = catapult_mpc_.Next();
+
+ if (!solution.has_value()) {
+ CHECK_NOTNULL(catapult_voltage);
+ *catapult_voltage = 0.0;
+ if (catapult_mpc_.started()) {
+ // Finished the catapult, time to fire.
+ catapult_state_ = CatapultState::RESETTING;
+ }
+ } else {
+ // TODO(austin): Voltage error?
+ CHECK_NOTNULL(catapult_voltage);
+ *catapult_voltage =
+ std::max(0.0, std::min(12.0, *solution - 0.0 * next_X(2, 0)));
+ use_profile_ = false;
+ }
+ } else {
+ if (unsafe_goal && unsafe_goal->has_catapult() &&
+ !unsafe_goal->catapult()->fire()) {
+ // Eh, didn't manage to solve before it was time to fire. Give up.
+ catapult_state_ = CatapultState::PROFILE;
+ }
+ }
+
+ if (!use_profile_ || catapult_state_ == CatapultState::RESETTING) {
+ catapult_.ForceGoal(catapult_.estimated_position(),
+ catapult_.estimated_velocity());
+ }
+ } break;
+
+ case CatapultState::RESETTING:
+ if (catapult_.controller().R(1, 0) > 0.0) {
+ catapult_.AdjustProfile(7.0, 500.0);
+ } else {
+ catapult_state_ = CatapultState::PROFILE;
+ }
+ [[fallthrough]];
+
+ case CatapultState::PROFILE:
+ break;
+ }
+
+ if (use_profile_) {
+ if (catapult_state_ != CatapultState::FIRING) {
+ catapult_mpc_.Reset();
+ }
+ // Select the controller designed for when we have no ball.
+ catapult_.set_controller_index(1);
+
+ const double output_voltage = catapult_.UpdateController(catapult_disabled);
+ if (catapult_voltage != nullptr) {
+ *catapult_voltage = output_voltage;
+ }
+ }
+
+ catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage : 0.0);
+
+ return catapult_.MakeStatus(fbb);
+}
+
} // namespace catapult
} // namespace superstructure
} // namespace control_loops
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index 85e9242..a5ffc92 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -5,6 +5,10 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "glog/logging.h"
#include "osqp++.h"
+#include "y2022/constants.h"
+#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
namespace y2022 {
namespace control_loops {
@@ -57,6 +61,8 @@
Eigen::Matrix<double, 2, 1> X_initial_;
Eigen::Matrix<double, 2, 1> X_final_;
+ Eigen::Matrix<double, Eigen::Dynamic, 1> objective_vector_;
+
// Solver state.
osqp::OsqpInstance instance_;
osqp::OsqpSolver solver_;
@@ -124,6 +130,97 @@
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Wmpw_;
};
+// A class to hold all the state needed to manage the catapult MPC solvers for
+// repeated shots.
+//
+// The solver may take a couple of cycles to get everything converged and ready.
+// The flow is as follows:
+// 1) Reset() the state for the new problem.
+// 2) Update to the current state with SetState()
+// 3) Call Solve(). This will return true if it is ready to be executed, false
+// if it needs more iterations to fully converge.
+// 4) Next() returns the current optimal control output and advances the
+// pointers to the next problem.
+// 5) Go back to 2 for the next cycle.
+class CatapultController {
+ public:
+ CatapultController(size_t horizon);
+
+ // Starts back over at the first controller.
+ void Reset();
+
+ // Updates our current and final states for the current controller.
+ void SetState(Eigen::Matrix<double, 2, 1> X_initial,
+ Eigen::Matrix<double, 2, 1> X_final);
+
+ // Solves! Returns true if the solution converged and osqp was happy.
+ bool Solve();
+
+ // Returns the time in seconds it last took Solve to run.
+ double solve_time() const { return solve_time_; }
+
+ // 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.
+ std::optional<double> Next();
+
+ // Returns true if Next has been called and a controller has been used. Reset
+ // starts over.
+ bool started() const { return current_controller_ != 0u; }
+
+ private:
+ CatapultProblemGenerator generator_;
+
+ std::vector<std::unique_ptr<MPCProblem>> problems_;
+
+ size_t current_controller_ = 0;
+ double solve_time_ = 0.0;
+};
+
+// Class to handle transitioning between both the profiled subsystem and the MPC
+// for shooting.
+class Catapult {
+ public:
+ Catapult(const constants::Values &values)
+ : catapult_(values.catapult.subsystem_params), catapult_mpc_(35) {}
+
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+ // Resets all state for when WPILib restarts.
+ void Reset() { catapult_.Reset(); }
+
+ bool zeroed() const { return catapult_.zeroed(); }
+ bool estopped() const { return catapult_.estopped(); }
+ double solve_time() const { return catapult_mpc_.solve_time(); }
+
+ bool mpc_active() const { return !use_profile_; }
+
+ // Runs either the MPC or the profiled subsystem depending on if we are
+ // shooting or not. Returns the status.
+ const flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ Iterate(const Goal *unsafe_goal, const Position *position,
+ double *catapult_voltage, flatbuffers::FlatBufferBuilder *fbb);
+
+ private:
+ // TODO(austin): Prototype is just an encoder. Catapult has both an encoder
+ // and pot. Switch back once we have a catapult.
+ // PotAndAbsoluteEncoderSubsystem catapult_;
+ PotAndAbsoluteEncoderSubsystem catapult_;
+
+ catapult::CatapultController catapult_mpc_;
+
+ enum CatapultState { PROFILE, FIRING, RESETTING };
+
+ CatapultState catapult_state_ = CatapultState::PROFILE;
+
+ bool last_firing_ = false;
+ bool use_profile_ = true;
+};
+
} // namespace catapult
} // namespace superstructure
} // namespace control_loops
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index bf04774..a1e3f13 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -15,14 +15,15 @@
const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
-
- climber_(values->climber.subsystem_params),
- intake_front_(values->intake_front.subsystem_params),
- intake_back_(values->intake_back.subsystem_params),
- turret_(values->turret.subsystem_params),
+ values_(values),
+ climber_(values_->climber.subsystem_params),
+ intake_front_(values_->intake_front.subsystem_params),
+ intake_back_(values_->intake_back.subsystem_params),
+ turret_(values_->turret.subsystem_params),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
- "/drivetrain")) {
+ "/drivetrain")),
+ catapult_(*values_) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -30,12 +31,15 @@
const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
+ OutputT output_struct;
+
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
intake_front_.Reset();
intake_back_.Reset();
turret_.Reset();
climber_.Reset();
+ catapult_.Reset();
}
drivetrain_status_fetcher_.Fetch();
@@ -57,7 +61,12 @@
transfer_roller_speed = unsafe_goal->transfer_roller_speed();
}
- OutputT output_struct;
+
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ catapult_status_offset = catapult_.Iterate(
+ unsafe_goal, position,
+ output != nullptr ? &(output_struct.catapult_voltage) : nullptr,
+ status->fbb());
flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
climber_status_offset = climber_.Iterate(
@@ -98,9 +107,9 @@
Status::Builder status_builder = status->MakeBuilder<Status>();
const bool zeroed = intake_front_.zeroed() && intake_back_.zeroed() &&
- turret_.zeroed() && climber_.zeroed();
+ turret_.zeroed() && climber_.zeroed() && catapult_.zeroed();
const bool estopped = intake_front_.estopped() || intake_back_.estopped() ||
- turret_.estopped() || climber_.zeroed();
+ turret_.estopped() || climber_.estopped() || catapult_.estopped();
status_builder.add_zeroed(zeroed);
status_builder.add_estopped(estopped);
@@ -109,6 +118,9 @@
status_builder.add_intake_back(intake_status_offset_back);
status_builder.add_turret(turret_status_offset);
status_builder.add_climber(climber_status_offset);
+ status_builder.add_catapult(catapult_status_offset);
+ status_builder.add_solve_time(catapult_.solve_time());
+ status_builder.add_mpc_active(catapult_.mpc_active());
(void)status->Send(status_builder.Finish());
}
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
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 37e63b5..ada39a2 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -2,6 +2,19 @@
namespace y2022.control_loops.superstructure;
+table CatapultGoal {
+ // If true, fire! The robot will only fire when ready.
+ fire:bool (id: 0);
+
+ // The target shot position and velocity. If these are provided before fire
+ // is called, the optimizer can pre-compute the trajectory.
+ shot_position:double (id: 1);
+ shot_velocity:double (id: 2);
+
+ // The target position to return the catapult to when not shooting.
+ return_position:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
+}
+
table Goal {
// Height of the climber above rest point
climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
@@ -21,6 +34,9 @@
roller_speed_compensation:double (id: 6);
turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 7);
+
+ // Catapult goal state.
+ catapult:CatapultGoal (id: 8);
}
root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 2ddbc3e..7ab3c30 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,7 +7,12 @@
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.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/superstructure.h"
+#include "y2022/control_loops/superstructure/turret/turret_plant.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
@@ -133,8 +138,8 @@
event_loop_->MakeFetcher<Output>("/superstructure")),
intake_front_(new CappedTestPlant(intake::MakeIntakePlant()),
PositionSensorSimulator(
- values->intake_front.subsystem_params.zeroing_constants
- .one_revolution_distance),
+ values->intake_front.subsystem_params
+ .zeroing_constants.one_revolution_distance),
values->intake_front, constants::Values::kIntakeRange(),
values->intake_front.subsystem_params.zeroing_constants
.measured_absolute_position,
@@ -155,6 +160,14 @@
values->turret.subsystem_params.zeroing_constants
.measured_absolute_position,
dt_),
+ catapult_(new CappedTestPlant(catapult::MakeCatapultPlant()),
+ PositionSensorSimulator(
+ values->catapult.subsystem_params.zeroing_constants
+ .one_revolution_distance),
+ values->catapult, constants::Values::kCatapultRange(),
+ values->catapult.subsystem_params.zeroing_constants
+ .measured_absolute_position,
+ dt_),
climber_(new CappedTestPlant(climber::MakeClimberPlant()),
PositionSensorSimulator(
constants::Values::kClimberPotMetersPerRevolution()),
@@ -164,6 +177,7 @@
constants::Values::kIntakeRange().middle());
intake_back_.InitializePosition(constants::Values::kIntakeRange().middle());
turret_.InitializePosition(constants::Values::kTurretRange().middle());
+ catapult_.InitializePosition(constants::Values::kCatapultRange().middle());
climber_.InitializePosition(constants::Values::kClimberRange().middle());
phased_loop_handle_ = event_loop_->AddPhasedLoop(
@@ -181,6 +195,9 @@
superstructure_status_fetcher_->intake_back());
turret_.Simulate(superstructure_output_fetcher_->turret_voltage(),
superstructure_status_fetcher_->turret());
+ catapult_.Simulate(
+ superstructure_output_fetcher_->catapult_voltage(),
+ superstructure_status_fetcher_->catapult());
climber_.Simulate(superstructure_output_fetcher_->climber_voltage(),
superstructure_status_fetcher_->climber());
}
@@ -200,6 +217,11 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
turret_.encoder()->GetSensorValues(&turret_builder);
+ frc971::PotAndAbsolutePosition::Builder catapult_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> catapult_offset =
+ catapult_.encoder()->GetSensorValues(&catapult_builder);
+
frc971::PotAndAbsolutePosition::Builder intake_front_builder =
builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_front_offset =
@@ -220,6 +242,7 @@
position_builder.add_intake_front(intake_front_offset);
position_builder.add_intake_back(intake_back_offset);
position_builder.add_turret(turret_offset);
+ position_builder.add_catapult(catapult_offset);
position_builder.add_climber(climber_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
@@ -229,6 +252,7 @@
PotAndAbsoluteEncoderSimulator *intake_front() { return &intake_front_; }
PotAndAbsoluteEncoderSimulator *intake_back() { return &intake_back_; }
PotAndAbsoluteEncoderSimulator *turret() { return &turret_; }
+ PotAndAbsoluteEncoderSimulator *catapult() { return &catapult_; }
RelativeEncoderSimulator *climber() { return &climber_; }
private:
@@ -245,6 +269,7 @@
PotAndAbsoluteEncoderSimulator intake_front_;
PotAndAbsoluteEncoderSimulator intake_back_;
PotAndAbsoluteEncoderSimulator turret_;
+ PotAndAbsoluteEncoderSimulator catapult_;
RelativeEncoderSimulator climber_;
};
@@ -312,6 +337,15 @@
superstructure_status_fetcher_->turret()->position(), 0.001);
}
+ if (superstructure_goal_fetcher_->has_catapult() &&
+ superstructure_goal_fetcher_->catapult()->has_return_position()) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->catapult()
+ ->return_position()
+ ->unsafe_goal(),
+ superstructure_status_fetcher_->catapult()->position(),
+ 0.001);
+ }
+
if (superstructure_goal_fetcher_->has_climber()) {
EXPECT_NEAR(superstructure_goal_fetcher_->climber()->unsafe_goal(),
superstructure_status_fetcher_->climber()->position(), 0.001);
@@ -433,6 +467,8 @@
constants::Values::kIntakeRange().middle());
superstructure_plant_.turret()->InitializePosition(
constants::Values::kTurretRange().middle());
+ superstructure_plant_.catapult()->InitializePosition(
+ constants::Values::kCatapultRange().middle());
superstructure_plant_.climber()->InitializePosition(
constants::Values::kClimberRange().middle());
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
index 8de272b..ac6f094 100644
--- a/y2022/control_loops/superstructure/superstructure_position.fbs
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -14,11 +14,14 @@
flipper_arm_left:frc971.RelativePosition (id: 4);
flipper_arm_right:frc971.RelativePosition (id: 5);
-
- // True is broken
+ // True means there is a ball in front of the sensor.
intake_beambreak_front:bool (id:6);
intake_beambreak_back:bool (id:7);
turret_beambreak:bool (id:8);
+
+ // Position of the catapult. Positive is up to fire. Zero is horizontal
+ // with the drive base.
+ catapult:frc971.PotAndAbsolutePosition (id: 9);
}
root_type Position;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index b4bad2a..8033d8a 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -18,6 +18,11 @@
intake_back:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 4);
turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);
+
+ catapult:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6);
+
+ solve_time:double (id: 7);
+ mpc_active:bool (id: 8);
}
-root_type Status;
\ No newline at end of file
+root_type Status;
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:
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 032b686..2868539 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -107,6 +107,11 @@
static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
"medium encoders are too fast");
+double catapult_pot_translate(double voltage) {
+ return voltage * Values::kCatapultPotRatio() *
+ (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
} // namespace
// Class to send position messages with sensor readings to our loops.
@@ -137,10 +142,33 @@
autonomous_modes_.at(i) = ::std::move(sensor);
}
+ void set_catapult_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ catapult_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_catapult_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ catapult_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_catapult_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ catapult_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
void RunIteration() override {
{
auto builder = superstructure_position_sender_.MakeBuilder();
+ frc971::PotAndAbsolutePositionT catapult;
+ CopyPosition(catapult_encoder_, &catapult,
+ Values::kCatapultEncoderCountsPerRevolution(),
+ Values::kCatapultEncoderRatio(), catapult_pot_translate,
+ true, values_->catapult.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> catapult_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &catapult);
+
frc971::RelativePositionT climber;
CopyPosition(*climber_potentiometer_, &climber, climber_pot_translate,
false, values_->climber.potentiometer_offset);
@@ -197,6 +225,7 @@
intake_beambreak_front_->Get());
position_builder.add_intake_beambreak_back(intake_beambreak_back_->Get());
position_builder.add_turret_beambreak(turret_beambreak_->Get());
+ position_builder.add_catapult(catapult_offset);
builder.CheckOk(builder.Send(position_builder.Finish()));
}
@@ -325,6 +354,8 @@
flipper_arm_right_potentiometer_, flipper_arm_left_potentiometer_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_encoder_front_,
intake_encoder_back_, turret_encoder_;
+
+ frc971::wpilib::AbsoluteEncoderAndPotentiometer catapult_encoder_;
};
class SuperstructureWriter
@@ -511,33 +542,39 @@
sensor_reader.set_flipper_arm_right_potentiometer(
make_unique<frc::AnalogInput>(5));
+ sensor_reader.set_catapult_encoder(
+ make_unique<frc::Encoder>(0, 1, false, frc::Encoder::k4X));
+ sensor_reader.set_catapult_absolute_pwm(std::make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_catapult_potentiometer(std::make_unique<frc::AnalogInput>(2));
+
AddLoop(&sensor_reader_event_loop);
// Thread 4.
::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), true);
drivetrain_writer.set_right_controller0(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), false);
SuperstructureWriter superstructure_writer(&output_event_loop);
- superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(0));
- superstructure_writer.set_catapult_falcon_1(make_unique<::frc::TalonFX>(1));
- superstructure_writer.set_catapult_falcon_2(make_unique<::frc::TalonFX>(2));
+ superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(8));
superstructure_writer.set_roller_falcon_front(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(3));
superstructure_writer.set_roller_falcon_back(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(4));
superstructure_writer.set_transfer_roller_victor(
- make_unique<::frc::VictorSP>(2));
+ make_unique<::frc::VictorSP>(9));
superstructure_writer.set_intake_falcon_front(make_unique<frc::TalonFX>(5));
superstructure_writer.set_intake_falcon_back(make_unique<frc::TalonFX>(6));
superstructure_writer.set_climber_falcon(make_unique<frc::TalonFX>(7));
superstructure_writer.set_flipper_arms_falcon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(5));
+ superstructure_writer.set_catapult_falcon_1(make_unique<::frc::TalonFX>(2));
+ superstructure_writer.set_catapult_falcon_2(make_unique<::frc::TalonFX>(3));
+
AddLoop(&output_event_loop);
RunLoops();