Move shared flywheel code into frc971
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ieac317a3e5bc8243e63473f485a2467b74aed348
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 83af834..fc2c0b4 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -35,6 +35,7 @@
deps = [
"//frc971/control_loops:control_loops_ts_fbs",
"//frc971/control_loops:profiled_subsystem_ts_fbs",
+ "//frc971/control_loops/flywheel:flywheel_controller_status_ts_fbs",
],
)
@@ -44,11 +45,12 @@
"superstructure_status.fbs",
],
gen_reflections = 1,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops:profiled_subsystem_fbs_includes",
- ],
target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/flywheel:flywheel_controller_status_fbs",
+ ],
)
flatbuffer_cc_library(
@@ -133,6 +135,7 @@
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/flywheel:flywheel_test_plant",
"//y2020/control_loops/superstructure/hood:hood_plants",
"//y2020/control_loops/superstructure/intake:intake_plants",
"//y2020/control_loops/superstructure/shooter:shooter_plants",
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index 4b0bf03..82e9fec 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -20,30 +20,13 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
- ":flywheel_controller",
"//frc971/control_loops:control_loop",
"//frc971/control_loops:profiled_subsystem",
+ "//frc971/control_loops/flywheel:flywheel_controller",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
- ],
-)
-
-cc_library(
- name = "flywheel_controller",
- srcs = [
- "flywheel_controller.cc",
- ],
- hdrs = [
- "flywheel_controller.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- "//frc971/control_loops:control_loop",
- "//frc971/control_loops:profiled_subsystem",
- "//y2020/control_loops/superstructure:superstructure_goal_fbs",
- "//y2020/control_loops/superstructure:superstructure_status_fbs",
"//y2020/control_loops/superstructure/accelerator:accelerator_plants",
"//y2020/control_loops/superstructure/finisher:finisher_plants",
],
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
deleted file mode 100644
index a25ed53..0000000
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ /dev/null
@@ -1,175 +0,0 @@
-#include "y2020/control_loops/superstructure/shooter/flywheel_controller.h"
-
-#include <chrono>
-
-#include "aos/logging/logging.h"
-#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
-#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
-
-namespace y2020 {
-namespace control_loops {
-namespace superstructure {
-namespace shooter {
-
-// Class to current limit battery current for a flywheel controller.
-class CurrentLimitedStateFeedbackController
- : public StateFeedbackLoop<3, 1, 1, double,
- StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>> {
- public:
- // Builds a CurrentLimitedStateFeedbackController given the coefficients, bemf
- // coefficient (units of radians/sec / volt), and motor resistance in ohms.
- CurrentLimitedStateFeedbackController(
- StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>> &&other,
- double bemf, double resistance)
- : StateFeedbackLoop(std::move(other)),
- bemf_(bemf),
- resistance_(resistance) {}
-
- double resistance() const { return resistance_; }
- double bemf() const { return bemf_; }
-
- void CapU() override {
- const double bemf_voltage = X_hat(1) / bemf_;
- // Solve the system of equations:
- //
- // motor_current = (u - bemf_voltage) / resistance
- // battery_current = ((u - bemf_voltage) / resistance) * u / 12.0
- // 0.0 = u * u - u * bemf_voltage - max_current * 12.0 * resistance
- //
- // And we have a quadratic!
- const double a = 1;
- const double b = -bemf_voltage;
- const double c_positive = -70.0 * 12.0 * resistance_;
- const double c_negative = -25.0 * 12.0 * resistance_;
-
- // Root is always positive.
- const double root_positive = std::sqrt(b * b - 4.0 * a * c_positive);
- const double root_negative = std::sqrt(b * b - 4.0 * a * c_negative);
- const double upper_limit = (-b + root_positive) / (2.0 * a);
- const double lower_limit = (-b - root_negative) / (2.0 * a);
-
- // Limit to the battery voltage and the current limit voltage.
- mutable_U(0, 0) = std::clamp(U(0, 0), lower_limit, upper_limit);
- if (R(1) > 50.0) {
- mutable_U(0, 0) = std::clamp(U(0, 0), 1.0, 12.0);
- } else {
- mutable_U(0, 0) = std::clamp(U(0, 0), 0.0, 12.0);
- }
- }
-
- private:
- double bemf_ = 0.0;
- double resistance_ = 0.0;
-};
-
-FlywheelController::FlywheelController(
- StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>> &&loop,
- double bemf, double resistance)
- : loop_(new CurrentLimitedStateFeedbackController(std::move(loop), bemf,
- resistance)) {
- history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
- 0, ::aos::monotonic_clock::epoch()));
- Y_.setZero();
-}
-
-void FlywheelController::set_goal(double angular_velocity_goal) {
- loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
- last_goal_ = angular_velocity_goal;
-}
-
-void FlywheelController::set_position(
- double current_position,
- const aos::monotonic_clock::time_point position_timestamp) {
- // Project time forwards.
- const int newest_history_position =
- ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
-
- if (!first_) {
- loop_->UpdateObserver(
- loop_->U(),
- position_timestamp - std::get<1>(history_[newest_history_position]));
- } else {
- first_ = false;
- }
-
- // Update position in the model.
- Y_ << current_position;
-
- // Add the position to the history.
- history_[history_position_] =
- std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
- position_timestamp);
- history_position_ = (history_position_ + 1) % kHistoryLength;
-
- loop_->Correct(Y_);
-}
-
-double FlywheelController::voltage() const { return loop_->U(0, 0); }
-
-double FlywheelController::current() const {
- return ((voltage() - (velocity() / loop_->bemf())) / (loop_->resistance())) *
- voltage() / 12.0;
-}
-
-void FlywheelController::Update(bool disabled) {
- loop_->mutable_R() = loop_->next_R();
- if (loop_->R(1, 0) < 1.0) {
- // Kill power at low angular velocities.
- disabled = true;
- }
-
- loop_->UpdateController(disabled);
-}
-
-flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
- flatbuffers::FlatBufferBuilder *fbb) {
- // Compute the oldest point in the history.
- const int oldest_history_position = history_position_;
- const int newest_history_position =
- ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
- const int second_newest_history_position =
- ((newest_history_position == 0) ? kHistoryLength
- : newest_history_position) -
- 1;
-
- const double total_loop_time = ::aos::time::DurationInSeconds(
- std::get<1>(history_[newest_history_position]) -
- std::get<1>(history_[oldest_history_position]));
-
- const double distance_traveled =
- std::get<0>(history_[newest_history_position]) -
- std::get<0>(history_[oldest_history_position]);
-
- const double last_loop_time = ::aos::time::DurationInSeconds(
- std::get<1>(history_[newest_history_position]) -
- std::get<1>(history_[second_newest_history_position]));
-
- const double last_distance_traveled =
- std::get<0>(history_[newest_history_position]) -
- std::get<0>(history_[second_newest_history_position]);
-
- // Compute the distance moved over that time period.
- avg_angular_velocity_ = (distance_traveled) / (total_loop_time);
-
- FlywheelControllerStatusBuilder builder(*fbb);
-
- builder.add_avg_angular_velocity(avg_angular_velocity_);
- builder.add_dt_angular_velocity(last_distance_traveled / last_loop_time);
- builder.add_angular_velocity(loop_->X_hat(1, 0));
- builder.add_voltage_error(loop_->X_hat(2, 0));
- builder.add_commanded_current(current());
- builder.add_angular_velocity_goal(last_goal_);
- return builder.Finish();
-}
-
-FlywheelController::~FlywheelController() {}
-
-double FlywheelController::velocity() const { return loop_->X_hat(1, 0); }
-
-} // namespace shooter
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2020
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
deleted file mode 100644
index 1d56407..0000000
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ /dev/null
@@ -1,83 +0,0 @@
-#ifndef Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
-#define Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
-
-#include <memory>
-
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
-#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
-#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2020 {
-namespace control_loops {
-namespace superstructure {
-namespace shooter {
-
-class CurrentLimitedStateFeedbackController;
-
-// Handles the velocity control of each flywheel.
-class FlywheelController {
- public:
- FlywheelController(
- StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>> &&loop,
- double bemf, double resistance);
-
- ~FlywheelController();
-
- // Sets the velocity goal in radians/sec
- void set_goal(double angular_velocity_goal);
- double goal() const { return last_goal_; }
- // Sets the current encoder position in radians
- void set_position(double current_position,
- const aos::monotonic_clock::time_point position_timestamp);
-
- // Populates the status structure.
- flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
- flatbuffers::FlatBufferBuilder *fbb);
-
- // Returns the control loop calculated voltage.
- double voltage() const;
-
- // Returns the expected battery current for the last U.
- double current() const;
-
- // Returns the instantaneous velocity.
- double velocity() const;
-
- // Executes the control loop for a cycle.
- void Update(bool disabled);
-
- double avg_angular_velocity() { return avg_angular_velocity_; }
-
- private:
- // The current sensor measurement.
- Eigen::Matrix<double, 1, 1> Y_;
- // The control loop.
- ::std::unique_ptr<CurrentLimitedStateFeedbackController> loop_;
-
- // History array for calculating a filtered angular velocity.
- static constexpr int kHistoryLength = 10;
- ::std::array<std::pair<double, ::aos::monotonic_clock::time_point>,
- kHistoryLength>
- history_;
- ptrdiff_t history_position_ = 0;
-
- // Average velocity logging.
- double avg_angular_velocity_ = 0;
-
- double last_goal_ = 0;
-
- bool first_ = true;
-
- DISALLOW_COPY_AND_ASSIGN(FlywheelController);
-};
-
-} // namespace shooter
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2020
-
-#endif // Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 9be6273..c400e34 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -5,7 +5,9 @@
#include "aos/logging/logging.h"
#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
+#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
namespace y2020 {
namespace control_loops {
@@ -78,11 +80,11 @@
UpToSpeed(goal);
}
- flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
- finisher_.SetStatus(fbb);
- flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
- accelerator_left_.SetStatus(fbb);
- flatbuffers::Offset<FlywheelControllerStatus>
+ flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
+ finisher_status_offset = finisher_.SetStatus(fbb);
+ flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
+ accelerator_left_status_offset = accelerator_left_.SetStatus(fbb);
+ flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
accelerator_right_status_offset = accelerator_right_.SetStatus(fbb);
ShooterStatusBuilder status_builder(*fbb);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index ccbb326..a1d7be3 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -2,8 +2,8 @@
#define Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/shooter/flywheel_controller.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
@@ -42,7 +42,8 @@
// flywheel and when it gets shot.
static constexpr double kMinFinisherVelocityDipWithBall = 5.0;
- FlywheelController finisher_, accelerator_left_, accelerator_right_;
+ frc971::control_loops::flywheel::FlywheelController finisher_,
+ accelerator_left_, accelerator_right_;
void UpToSpeed(const ShooterGoal *goal);
bool finisher_ready_ = false;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index d2051df..8b34fc9 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -11,6 +11,7 @@
#include "aos/network/team_number.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/flywheel/flywheel_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "y2020/constants.h"
@@ -51,39 +52,6 @@
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
-class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
- public:
- explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other, double bemf,
- double resistance)
- : StateFeedbackPlant<2, 1, 1>(::std::move(other)),
- bemf_(bemf),
- resistance_(resistance) {}
-
- void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
- EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
- EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
- }
-
- double motor_current(const Eigen::Matrix<double, 1, 1> U) const {
- return (U(0) - X(1) / bemf_) / resistance_;
- }
-
- double battery_current(const Eigen::Matrix<double, 1, 1> U) const {
- return motor_current(U) * U(0) / 12.0;
- }
-
- double voltage_offset() const { return voltage_offset_; }
- void set_voltage_offset(double voltage_offset) {
- voltage_offset_ = voltage_offset;
- }
-
- private:
- double voltage_offset_ = 0.0;
-
- double bemf_;
- double resistance_;
-};
-
// Class which simulates the superstructure and sends out queue messages with
// the position.
class SuperstructureSimulation {
@@ -111,14 +79,16 @@
.turret.subsystem_params.zeroing_constants
.one_revolution_distance),
accelerator_left_plant_(
- new FlywheelPlant(accelerator::MakeAcceleratorPlant(),
- accelerator::kBemf, accelerator::kResistance)),
+ new frc971::control_loops::flywheel::FlywheelPlant(
+ accelerator::MakeAcceleratorPlant(), accelerator::kBemf,
+ accelerator::kResistance)),
accelerator_right_plant_(
- new FlywheelPlant(accelerator::MakeAcceleratorPlant(),
- accelerator::kBemf, accelerator::kResistance)),
- finisher_plant_(new FlywheelPlant(finisher::MakeFinisherPlant(),
- finisher::kBemf,
- finisher::kResistance)) {
+ new frc971::control_loops::flywheel::FlywheelPlant(
+ accelerator::MakeAcceleratorPlant(), accelerator::kBemf,
+ accelerator::kResistance)),
+ finisher_plant_(new frc971::control_loops::flywheel::FlywheelPlant(
+ finisher::MakeFinisherPlant(), finisher::kBemf,
+ finisher::kResistance)) {
InitializeHoodPosition(constants::Values::kHoodRange().upper);
InitializeIntakePosition(constants::Values::kIntakeRange().upper);
InitializeTurretPosition(constants::Values::kTurretRange().middle());
@@ -413,9 +383,12 @@
::std::unique_ptr<CappedTestPlant> turret_plant_;
PositionSensorSimulator turret_encoder_;
- ::std::unique_ptr<FlywheelPlant> accelerator_left_plant_;
- ::std::unique_ptr<FlywheelPlant> accelerator_right_plant_;
- ::std::unique_ptr<FlywheelPlant> finisher_plant_;
+ ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+ accelerator_left_plant_;
+ ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+ accelerator_right_plant_;
+ ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+ finisher_plant_;
// The acceleration limits to check for while moving.
double peak_hood_acceleration_ = 1e10;
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 611661c..d3f98e5 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -1,5 +1,6 @@
include "frc971/control_loops/control_loops.fbs";
include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/control_loops/flywheel/flywheel_controller_status.fbs";
namespace y2020.control_loops.superstructure;
@@ -9,36 +10,14 @@
TURRET
}
-table FlywheelControllerStatus {
- // The current average velocity in radians/second over the last kHistoryLength
- // in shooter.h
- avg_angular_velocity:double (id: 0);
-
- // The current instantaneous filtered velocity in radians/second.
- angular_velocity:double (id: 1);
-
- // The target speed selected by the lookup table or from manual override
- // Can be compared to velocity to determine if ready.
- angular_velocity_goal:double (id: 2);
-
- // Voltage error.
- voltage_error:double (id: 3);
-
- // The commanded battery current.
- commanded_current:double (id: 4);
-
- // The angular velocity of the flywheel computed using delta x / delta t
- dt_angular_velocity:double (id: 5);
-}
-
table ShooterStatus {
// The final wheel shooting the ball
- finisher:FlywheelControllerStatus (id: 0);
+ finisher:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 0);
// The subsystem to accelerate the ball before the finisher
// Velocity is the fastest (top) wheel
- accelerator_left:FlywheelControllerStatus (id: 1);
- accelerator_right:FlywheelControllerStatus (id: 2);
+ accelerator_left:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 1);
+ accelerator_right:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 2);
// If the shooter is ready, this is true. Note: don't use this for status
// checking, only for plotting. Changes in goal take time to impact this.