Add a current limit on the flywheels.
We were blowing breakers. Limit the request to do better.
Change-Id: Ibd59d9c54e02991070d0d68f42eb33c29c3a4132
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 07c3d99..a8921d9 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -11,12 +11,56 @@
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) {}
+
+ 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 = -50.0 * 12.0 * resistance_;
+
+ // Root is always positive.
+ const double root = std::sqrt(b * b - 4.0 * a * c);
+ const double upper_limit = (-b + root) / (2.0 * a);
+ const double lower_limit = (-b - root) / (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);
+ mutable_U(0, 0) = std::clamp(U(0, 0), -12.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)
- : loop_(new StateFeedbackLoop<3, 1, 1, double,
- StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>>(std::move(loop))) {
+ 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();
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index e130389..d5f7ede 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -20,7 +20,8 @@
public:
FlywheelController(
StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>> &&loop);
+ HybridKalman<3, 1, 1>> &&loop,
+ double bemf, double resistance);
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index a9f1c4c..6e48a21 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -16,9 +16,12 @@
} // namespace
Shooter::Shooter()
- : finisher_(finisher::MakeIntegralFinisherLoop()),
- accelerator_left_(accelerator::MakeIntegralAcceleratorLoop()),
- accelerator_right_(accelerator::MakeIntegralAcceleratorLoop()) {}
+ : finisher_(finisher::MakeIntegralFinisherLoop(), finisher::kBemf,
+ finisher::kResistance),
+ accelerator_left_(accelerator::MakeIntegralAcceleratorLoop(),
+ accelerator::kBemf, accelerator::kResistance),
+ accelerator_right_(accelerator::MakeIntegralAcceleratorLoop(),
+ accelerator::kBemf, accelerator::kResistance) {}
bool Shooter::UpToSpeed(const ShooterGoal *goal) {
return (