Move shared flywheel code into frc971
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ieac317a3e5bc8243e63473f485a2467b74aed348
diff --git a/frc971/control_loops/flywheel/BUILD b/frc971/control_loops/flywheel/BUILD
new file mode 100644
index 0000000..6399abe
--- /dev/null
+++ b/frc971/control_loops/flywheel/BUILD
@@ -0,0 +1,49 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+flatbuffer_cc_library(
+ name = "flywheel_controller_status_fbs",
+ srcs = [
+ "flywheel_controller_status.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+ name = "flywheel_controller_status_ts_fbs",
+ srcs = [
+ "flywheel_controller_status.fbs",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "flywheel_test_plant",
+ hdrs = [
+ "flywheel_test_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":flywheel_controller",
+ ],
+)
+
+cc_library(
+ name = "flywheel_controller",
+ srcs = [
+ "flywheel_controller.cc",
+ ],
+ hdrs = [
+ "flywheel_controller.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":flywheel_controller_status_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ ],
+)
diff --git a/frc971/control_loops/flywheel/flywheel_controller.cc b/frc971/control_loops/flywheel/flywheel_controller.cc
new file mode 100644
index 0000000..adea0e8
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller.cc
@@ -0,0 +1,170 @@
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
+
+#include <chrono>
+
+#include "aos/logging/logging.h"
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+
+// 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 flywheel
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/flywheel/flywheel_controller.h b/frc971/control_loops/flywheel/flywheel_controller.h
new file mode 100644
index 0000000..db8baeb
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller.h
@@ -0,0 +1,80 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+
+#include <memory>
+
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/flywheel/flywheel_controller_status_generated.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+
+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 flywheel
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
diff --git a/frc971/control_loops/flywheel/flywheel_controller_status.fbs b/frc971/control_loops/flywheel/flywheel_controller_status.fbs
new file mode 100644
index 0000000..0f95818
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller_status.fbs
@@ -0,0 +1,22 @@
+namespace frc971.control_loops.flywheel;
+
+table FlywheelControllerStatus {
+ // The current average velocity in radians/second
+ 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);
+}
diff --git a/frc971/control_loops/flywheel/flywheel_test_plant.h b/frc971/control_loops/flywheel/flywheel_test_plant.h
new file mode 100644
index 0000000..01e347d
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_test_plant.h
@@ -0,0 +1,47 @@
+#ifndef FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
+#define FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
+
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+
+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_;
+};
+
+} // namespace flywheel
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 0066b93..699eb5a 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -247,6 +247,21 @@
)
py_binary(
+ name = "flywheel",
+ srcs = [
+ "flywheel.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:controls",
+ "@pip//matplotlib",
+ "@pip//pygobject",
+ ],
+)
+
+py_binary(
name = "static_zeroing_single_dof_profiled_subsystem_test",
srcs = [
"static_zeroing_single_dof_profiled_subsystem_test.py",
diff --git a/frc971/control_loops/python/flywheel.py b/frc971/control_loops/python/flywheel.py
new file mode 100644
index 0000000..1280b36
--- /dev/null
+++ b/frc971/control_loops/python/flywheel.py
@@ -0,0 +1,329 @@
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+
+import matplotlib
+import matplotlib.pyplot as plt
+
+import glog
+
+
+class FlywheelParams(object):
+
+ def __init__(self,
+ name,
+ motor,
+ G,
+ J,
+ q_pos,
+ q_vel,
+ q_voltage,
+ r_pos,
+ controller_poles,
+ dt=0.00505):
+ self.name = name
+ self.motor = motor
+ self.G = G
+ self.J = J
+ self.q_pos = q_pos
+ self.q_vel = q_vel
+ self.q_voltage = q_voltage
+ self.r_pos = r_pos
+ self.dt = dt
+ self.controller_poles = controller_poles
+
+
+class VelocityFlywheel(control_loop.HybridControlLoop):
+
+ def __init__(self, params, name="Flywheel"):
+ super(VelocityFlywheel, self).__init__(name=name)
+ self.params = params
+ # Set Motor
+ self.motor = self.params.motor
+ # Moment of inertia of the flywheel wheel in kg m^2
+ self.J = self.params.J
+ # Gear ratio
+ self.G = self.params.G
+ # Control loop time step
+ self.dt = self.params.dt
+
+ # State feedback matrices
+ # [angular velocity]
+ self.A_continuous = numpy.matrix([[
+ -self.motor.Kt / self.motor.Kv /
+ (self.J * self.G * self.G * self.motor.resistance)
+ ]])
+ self.B_continuous = numpy.matrix(
+ [[self.motor.Kt / (self.J * self.G * self.motor.resistance)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles(self.params.controller_poles)
+
+ # Generated controller not used.
+ self.PlaceObserverPoles([0.3])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+ glog.debug('K: %s', str(self.K))
+ glog.debug('Poles: %s',
+ str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
+
+class Flywheel(VelocityFlywheel):
+
+ def __init__(self, params, name="Flywheel"):
+ super(Flywheel, self).__init__(params, name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+ self.A_continuous[0, 1] = 1
+
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ rpl = 0.45
+ ipl = 0.07
+ self.PlaceObserverPoles([rpl + 1j * ipl, rpl - 1j * ipl])
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.K[0, 1:2] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+ self.Kff[0, 1:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
+
+class IntegralFlywheel(Flywheel):
+
+ def __init__(self, params, name="IntegralFlywheel"):
+ super(IntegralFlywheel, self).__init__(params, name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+ # states
+ # [position, velocity, voltage_error]
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
+
+ glog.debug('A_continuous %s' % str(self.A_continuous))
+ glog.debug('B_continuous %s' % str(self.B_continuous))
+ glog.debug('C %s' % str(self.C))
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ glog.debug('A %s' % str(self.A))
+ glog.debug('B %s' % str(self.B))
+
+ q_pos = self.params.q_pos
+ q_vel = self.params.q_vel
+ q_voltage = self.params.q_voltage
+ self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**2.0)]])
+
+ r_pos = self.params.r_pos
+ self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
+
+ _, _, self.Q, self.R = controls.kalmd(A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
+
+ glog.debug('Q_discrete %s' % (str(self.Q)))
+ glog.debug('R_discrete %s' % (str(self.R)))
+
+ self.KalmanGain, self.P_steady_state = controls.kalman(A=self.A,
+ B=self.B,
+ C=self.C,
+ Q=self.Q,
+ R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
+
+def PlotSpinup(params, goal, iterations=400):
+ """Runs the flywheel plant with an initial condition and goal.
+
+ Args:
+ flywheel: Flywheel object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_flywheel: Flywheel object to get K from, or None if we should
+ use flywheel.
+ observer_flywheel: Flywheel object to use for the observer, or None if we
+ should use the actual state.
+ """
+
+ # Various lists for graphing things.
+ t = []
+ x = []
+ v = []
+ a = []
+ x_hat = []
+ u = []
+ offset = []
+
+ flywheel = Flywheel(params, params.name)
+ controller_flywheel = IntegralFlywheel(params, params.name)
+ observer_flywheel = IntegralFlywheel(params, params.name)
+ vbat = 12.0
+
+ if t:
+ initial_t = t[-1] + flywheel.dt
+ else:
+ initial_t = 0
+
+ for i in range(iterations):
+ X_hat = flywheel.X
+
+ if observer_flywheel is not None:
+ X_hat = observer_flywheel.X_hat
+ x_hat.append(observer_flywheel.X_hat[1, 0])
+
+ ff_U = controller_flywheel.Kff * (goal - observer_flywheel.A * goal)
+
+ U = controller_flywheel.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ x.append(flywheel.X[0, 0])
+
+ if v:
+ last_v = v[-1]
+ else:
+ last_v = 0
+
+ v.append(flywheel.X[1, 0])
+ a.append((v[-1] - last_v) / flywheel.dt)
+
+ if observer_flywheel is not None:
+ observer_flywheel.Y = flywheel.Y
+ observer_flywheel.CorrectHybridObserver(U)
+ offset.append(observer_flywheel.X_hat[2, 0])
+
+ applied_U = U.copy()
+ if i > 200:
+ applied_U += 2
+ flywheel.Update(applied_U)
+
+ if observer_flywheel is not None:
+ observer_flywheel.PredictHybridObserver(U, flywheel.dt)
+
+ t.append(initial_t + i * flywheel.dt)
+ u.append(U[0, 0])
+
+ matplotlib.use("GTK3Agg")
+
+ plt.subplot(3, 1, 1)
+ plt.plot(t, v, label='x')
+ plt.plot(t, x_hat, label='x_hat')
+ plt.legend()
+
+ plt.subplot(3, 1, 2)
+ plt.plot(t, u, label='u')
+ plt.plot(t, offset, label='voltage_offset')
+ plt.legend()
+
+ plt.subplot(3, 1, 3)
+ plt.plot(t, a, label='a')
+ plt.legend()
+
+ plt.show()
+
+
+def WriteFlywheel(params, plant_files, controller_files, namespace):
+ """Writes out the constants for a flywheel to a file.
+
+ Args:
+ params: list of Flywheel Params, the
+ parameters defining the system.
+ plant_files: list of strings, the cc and h files for the plant.
+ controller_files: list of strings, the cc and h files for the integral
+ controller.
+ namespaces: list of strings, the namespace list to use.
+ """
+ # Write the generated constants out to a file.
+ flywheels = []
+ integral_flywheels = []
+
+ if type(params) is list:
+ name = params[0].name
+ for index, param in enumerate(params):
+ flywheels.append(Flywheel(param, name=param.name + str(index)))
+ integral_flywheels.append(
+ IntegralFlywheel(param,
+ name='Integral' + param.name + str(index)))
+ else:
+ name = params.name
+ flywheels.append(Flywheel(params, params.name))
+ integral_flywheels.append(
+ IntegralFlywheel(params, name='Integral' + params.name))
+
+ loop_writer = control_loop.ControlLoopWriter(name,
+ flywheels,
+ namespaces=namespace)
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', flywheels[0].G))
+ 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(
+ 'Integral' + name,
+ integral_flywheels,
+ namespaces=namespace,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
+ integral_loop_writer.Write(controller_files[0], controller_files[1])