Add a current limit on the flywheels.
We were blowing breakers. Limit the request to do better.
Change-Id: Ibd59d9c54e02991070d0d68f42eb33c29c3a4132
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 9c36c90..81f7deb 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -5,20 +5,24 @@
class Constant(object):
- def __init__(self, name, formatt, value):
+ def __init__(self, name, formatt, value, comment=None):
self.name = name
self.formatt = formatt
self.value = value
self.formatToType = {}
self.formatToType['%f'] = "double"
self.formatToType['%d'] = "int"
+ if comment is None:
+ self.comment = ""
+ else:
+ self.comment = comment + "\n"
def Render(self, loop_type):
typestring = self.formatToType[self.formatt]
if loop_type == 'float' and typestring == 'double':
typestring = loop_type
- return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
- (typestring, self.name, self.value)
+ return str("\n%sstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (self.comment, typestring, self.name, self.value)
class ControlLoopWriter(object):
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 451788a..f284450 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -295,6 +295,18 @@
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f',
flywheels[0].motor.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant(
+ 'kBemf',
+ '%f',
+ flywheels[0].motor.Kv * flywheels[0].G,
+ comment="// Radians/sec / volt"))
+ loop_writer.AddConstant(
+ control_loop.Constant(
+ 'kResistance',
+ '%f',
+ flywheels[0].motor.resistance,
+ comment="// Ohms"))
loop_writer.Write(plant_files[0], plant_files[1])
integral_loop_writer = control_loop.ControlLoopWriter(
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 (
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index e8f1f8d..1d42d23 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -43,14 +43,25 @@
class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
public:
- explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other)
- : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+ 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;
@@ -58,6 +69,9 @@
private:
double voltage_offset_ = 0.0;
+
+ double bemf_;
+ double resistance_;
};
// Class which simulates the superstructure and sends out queue messages with
@@ -84,10 +98,14 @@
.turret.subsystem_params.zeroing_constants
.one_revolution_distance),
accelerator_left_plant_(
- new FlywheelPlant(accelerator::MakeAcceleratorPlant())),
+ new FlywheelPlant(accelerator::MakeAcceleratorPlant(),
+ accelerator::kBemf, accelerator::kResistance)),
accelerator_right_plant_(
- new FlywheelPlant(accelerator::MakeAcceleratorPlant())),
- finisher_plant_(new FlywheelPlant(finisher::MakeFinisherPlant())) {
+ new FlywheelPlant(accelerator::MakeAcceleratorPlant(),
+ accelerator::kBemf, accelerator::kResistance)),
+ finisher_plant_(new FlywheelPlant(finisher::MakeFinisherPlant(),
+ finisher::kBemf,
+ finisher::kResistance)) {
InitializeHoodPosition(constants::Values::kHoodRange().upper);
InitializeIntakePosition(constants::Values::kIntakeRange().upper);
InitializeTurretPosition(constants::Values::kTurretRange().middle());
@@ -264,15 +282,26 @@
<< superstructure_output_fetcher_->accelerator_left_voltage() +
accelerator_left_plant_->voltage_offset();
+ // Confirm that we aren't drawing too much current.
+ CHECK_NEAR(accelerator_left_plant_->battery_current(accelerator_left_U),
+ 0.0, 60.0);
+
::Eigen::Matrix<double, 1, 1> accelerator_right_U;
accelerator_right_U
<< superstructure_output_fetcher_->accelerator_right_voltage() +
accelerator_right_plant_->voltage_offset();
+ // Confirm that we aren't drawing too much current.
+ CHECK_NEAR(accelerator_right_plant_->battery_current(accelerator_right_U),
+ 0.0, 60.0);
+
::Eigen::Matrix<double, 1, 1> finisher_U;
finisher_U << superstructure_output_fetcher_->finisher_voltage() +
finisher_plant_->voltage_offset();
+ // Confirm that we aren't drawing too much current.
+ CHECK_NEAR(finisher_plant_->battery_current(finisher_U), 0.0, 60.0);
+
hood_plant_->Update(hood_U);
intake_plant_->Update(intake_U);
turret_plant_->Update(turret_U);