Move shared flywheel code into frc971
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ieac317a3e5bc8243e63473f485a2467b74aed348
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
index 3833536..0650421 100644
--- a/y2020/control_loops/python/BUILD
+++ b/y2020/control_loops/python/BUILD
@@ -113,19 +113,6 @@
],
)
-py_library(
- name = "flywheel",
- srcs = [
- "flywheel.py",
- ],
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- "//frc971/control_loops/python:controls",
- "@pip//matplotlib",
- "@pip//pygobject",
- ],
-)
-
py_binary(
name = "accelerator",
srcs = [
@@ -134,8 +121,8 @@
legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
- ":flywheel",
":python_init",
+ "//frc971/control_loops/python:flywheel",
"@pip//glog",
"@pip//python_gflags",
],
@@ -149,8 +136,8 @@
legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
- ":flywheel",
":python_init",
+ "//frc971/control_loops/python:flywheel",
"@pip//glog",
"@pip//python_gflags",
],
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 54ec9d3..752f8b5 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -1,7 +1,7 @@
#!/usr/bin/python3
from frc971.control_loops.python import control_loop
-from y2020.control_loops.python import flywheel
+from frc971.control_loops.python import flywheel
import numpy
import sys
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 7e703fc..29af431 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -1,7 +1,7 @@
#!/usr/bin/python3
from frc971.control_loops.python import control_loop
-from y2020.control_loops.python import flywheel
+from frc971.control_loops.python import flywheel
import numpy
import sys
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
deleted file mode 100755
index 9153bc6..0000000
--- a/y2020/control_loops/python/flywheel.py
+++ /dev/null
@@ -1,325 +0,0 @@
-from frc971.control_loops.python import control_loop
-from frc971.control_loops.python import controls
-import numpy
-from matplotlib import pylab
-
-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])
-
- pylab.subplot(3, 1, 1)
- pylab.plot(t, v, label='x')
- pylab.plot(t, x_hat, label='x_hat')
- pylab.legend()
-
- pylab.subplot(3, 1, 2)
- pylab.plot(t, u, label='u')
- pylab.plot(t, offset, label='voltage_offset')
- pylab.legend()
-
- pylab.subplot(3, 1, 3)
- pylab.plot(t, a, label='a')
- pylab.legend()
-
- pylab.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])
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.