Add shooter class and python.
Change-Id: I27b4c8f282a0b80344a7df59cf3b04569d9c8110
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
index b9dfbe0..923d239 100644
--- a/y2020/control_loops/python/BUILD
+++ b/y2020/control_loops/python/BUILD
@@ -114,6 +114,48 @@
)
py_library(
+ name = "flywheel",
+ srcs = [
+ "flywheel.py",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ "//frc971/control_loops/python:controls",
+ "@matplotlib_repo//:matplotlib2.7",
+ ],
+)
+
+py_binary(
+ name = "accelerator",
+ srcs = [
+ "accelerator.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ ":flywheel",
+ "//external:python-gflags",
+ "//external:python-glog",
+ ],
+)
+
+py_binary(
+ name = "finisher",
+ srcs = [
+ "finisher.py",
+ ],
+ legacy_create_init = False,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":python_init",
+ ":flywheel",
+ "//external:python-gflags",
+ "//external:python-glog",
+ ],
+)
+
+py_library(
name = "python_init",
srcs = ["__init__.py"],
visibility = ["//visibility:public"],
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
new file mode 100644
index 0000000..257ed23
--- /dev/null
+++ b/y2020/control_loops/python/accelerator.py
@@ -0,0 +1,46 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from y2020.control_loops.python import flywheel
+import numpy
+
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+kAccelerator = flywheel.FlywheelParams(
+ name='Accelerator',
+ motor=control_loop.Falcon(),
+ G=1.0,
+ J=0.006,
+ q_pos=0.08,
+ q_vel=4.00,
+ q_voltage=0.3,
+ r_pos=0.05,
+ controller_poles=[.87],
+ dt=0.00505)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ flywheel.PlotSpinup(kAccelerator, goal=R, iterations=200)
+ return 0
+
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = [
+ 'y2020', 'control_loops', 'superstructure', 'accelerator'
+ ]
+ flywheel.WriteFlywheel(kAccelerator, argv[1:3], argv[3:5], namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
new file mode 100644
index 0000000..0b0fbb4
--- /dev/null
+++ b/y2020/control_loops/python/finisher.py
@@ -0,0 +1,44 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from y2020.control_loops.python import flywheel
+import numpy
+
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+kFinisher = flywheel.FlywheelParams(
+ name='Finisher',
+ motor=control_loop.Falcon(),
+ G=1.0,
+ J=0.006,
+ q_pos=0.08,
+ q_vel=4.00,
+ q_voltage=0.3,
+ r_pos=0.05,
+ controller_poles=[.87],
+ dt=0.00505)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
+ return 0
+
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2020', 'control_loops', 'superstructure', 'finisher']
+ flywheel.WriteFlywheel(kFinisher, argv[1:3], argv[3:5], namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
new file mode 100755
index 0000000..9a6fc46
--- /dev/null
+++ b/y2020/control_loops/python/flywheel.py
@@ -0,0 +1,285 @@
+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.ControlLoop):
+ 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)
+
+
+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
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ q_pos = self.params.q_pos
+ q_vel = self.params.q_vel
+ q_voltage = self.params.q_voltage
+ self.Q = 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 = numpy.matrix([[(r_pos**2.0)]])
+
+ self.KalmanGain, self.Q_steady = 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=200):
+ """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 xrange(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.CorrectObserver(U)
+ offset.append(observer_flywheel.X_hat[2, 0])
+
+ applied_U = U.copy()
+ if i > 30:
+ applied_U += 2
+ flywheel.Update(applied_U)
+
+ if observer_flywheel is not None:
+ observer_flywheel.PredictObserver(U)
+
+ t.append(initial_t + i * flywheel.dt)
+ u.append(U[0, 0])
+
+ glog.debug('Time: %f', t[-1])
+ 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' + params.name, '%f',
+ flywheels[0].G))
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed' + params.name, '%f',
+ flywheels[0].motor.free_speed))
+ loop_writer.Write(plant_files[0], plant_files[1])
+
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'Integral' + name, integral_flywheels, namespaces=namespace)
+ integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2020/control_loops/superstructure/accelerator/BUILD b/y2020/control_loops/superstructure/accelerator/BUILD
new file mode 100644
index 0000000..2c680bf
--- /dev/null
+++ b/y2020/control_loops/superstructure/accelerator/BUILD
@@ -0,0 +1,17 @@
+package(default_visibility = ["//visibility:public"])
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+genrule(
+ name = "genrule_accelerator",
+ outs = [
+ "accelerator_plant.h",
+ "accelerator_plant.cc",
+ "accelerator_integral_plant.h",
+ "accelerator_integral_plant.cc",
+ ],
+ cmd = "$(location //y2020/control_loops/python:accelerator) $(OUTS)",
+ tools = [
+ "//y2020/control_loops/python:accelerator",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/finisher/BUILD b/y2020/control_loops/superstructure/finisher/BUILD
new file mode 100644
index 0000000..b9bfc4f
--- /dev/null
+++ b/y2020/control_loops/superstructure/finisher/BUILD
@@ -0,0 +1,17 @@
+package(default_visibility = ["//visibility:public"])
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+genrule(
+ name = "genrule_finisher",
+ outs = [
+ "finisher_plant.h",
+ "finisher_plant.cc",
+ "finisher_integral_plant.h",
+ "finisher_integral_plant.cc",
+ ],
+ cmd = "$(location //y2020/control_loops/python:finisher) $(OUTS)",
+ tools = [
+ "//y2020/control_loops/python:finisher",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
new file mode 100644
index 0000000..d63231b
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -0,0 +1,58 @@
+package(default_visibility = ["//visibility:public"])
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+cc_library(
+ name = "shooter_plants",
+ srcs = [
+ "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.cc",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plant.cc",
+ "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.cc",
+ "//y2020/control_loops/superstructure/finisher:finisher_plant.cc",
+ ],
+ hdrs = [
+ "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.h",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plant.h",
+ "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.h",
+ "//y2020/control_loops/superstructure/finisher:finisher_plant.h",
+ ],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+cc_library(
+ name = "shooter",
+ srcs = [
+ "shooter.cc",
+ ],
+ hdrs = [
+ "shooter.h",
+ ],
+ deps = [
+ ":flywheel_controller",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ "//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",
+ ],
+ deps = [
+ ":shooter_plants",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2020/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
new file mode 100644
index 0000000..0f920a4
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -0,0 +1,70 @@
+#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 {
+
+FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
+ : loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
+ history_.fill(0);
+ 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) {
+ // Update position in the model.
+ Y_ << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+}
+
+double FlywheelController::voltage() const { return loop_->U(0, 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_->Correct(Y_);
+ loop_->Update(disabled);
+}
+
+flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ // Compute the oldest point in the history.
+ const int oldest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+ // Compute the distance moved over that time period.
+ const double avg_angular_velocity =
+ (history_[oldest_history_position] - history_[history_position_]) /
+ (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
+ static_cast<double>(kHistoryLength - 1));
+
+ FlywheelControllerStatusBuilder builder(*fbb);
+
+ builder.add_avg_angular_velocity(avg_angular_velocity);
+ builder.add_angular_velocity(loop_->X_hat(1, 0));
+ builder.add_angular_velocity_goal(last_goal_);
+ return builder.Finish();
+}
+
+} // 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
new file mode 100644
index 0000000..20cd681
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -0,0 +1,61 @@
+#ifndef Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+#define Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+
+#include <memory>
+
+#include "aos/controls/control_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_integral_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_integral_plant.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace shooter {
+
+// Handles the velocity control of each flywheel.
+class FlywheelController {
+ public:
+ FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop);
+
+ // Sets the velocity goal in radians/sec
+ void set_goal(double angular_velocity_goal);
+ // Sets the current encoder position in radians
+ void set_position(double current_position);
+
+ // Populates the status structure.
+ flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Returns the instantaneous velocity.
+ double velocity() const { return loop_->X_hat(1, 0); }
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
+
+ private:
+ // The current sensor measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // The control loop.
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+
+ // History array for calculating a filtered angular velocity.
+ static constexpr int kHistoryLength = 10;
+ ::std::array<double, kHistoryLength> history_;
+ ptrdiff_t history_position_ = 0;
+ double last_goal_ = 0;
+
+ 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
new file mode 100644
index 0000000..6a1d201
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -0,0 +1,62 @@
+#include "y2020/control_loops/superstructure/shooter/shooter.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 {
+
+Shooter::Shooter()
+ : finisher_(finisher::MakeIntegralFinisherLoop()),
+ accelerator_left_(accelerator::MakeIntegralAcceleratorLoop()),
+ accelerator_right_(accelerator::MakeIntegralAcceleratorLoop()) {}
+
+flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
+ const ShooterGoal *goal, const ShooterPosition *position,
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output) {
+ if (goal) {
+ // Update position/goal for our two shooter sides.
+ finisher_.set_goal(goal->velocity_finisher());
+ accelerator_left_.set_goal(goal->velocity_accelerator());
+ accelerator_right_.set_goal(goal->velocity_accelerator());
+ }
+
+ finisher_.set_position(position->theta_finisher());
+ accelerator_left_.set_position(position->theta_accelerator_left());
+ accelerator_right_.set_position(position->theta_accelerator_right());
+
+ finisher_.Update(output == nullptr);
+ accelerator_left_.Update(output == nullptr);
+ accelerator_right_.Update(output == nullptr);
+
+ flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
+ finisher_.SetStatus(fbb);
+ flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
+ accelerator_left_.SetStatus(fbb);
+ flatbuffers::Offset<FlywheelControllerStatus>
+ accelerator_right_status_offset = accelerator_right_.SetStatus(fbb);
+
+ ShooterStatusBuilder status_builder(*fbb);
+
+ status_builder.add_finisher(finisher_status_offset);
+ status_builder.add_accelerator_left(accelerator_left_status_offset);
+ status_builder.add_accelerator_right(accelerator_right_status_offset);
+
+ if (output) {
+ output->finisher_voltage = finisher_.voltage();
+ output->accelerator_left_voltage = accelerator_left_.voltage();
+ output->accelerator_right_voltage = accelerator_right_.voltage();
+ }
+
+ return status_builder.Finish();
+}
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
new file mode 100644
index 0000000..88bcb3b
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -0,0 +1,37 @@
+#ifndef Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+#define Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+
+#include "aos/controls/control_loop.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"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace shooter {
+
+// Handles all flywheels together.
+class Shooter {
+ public:
+ Shooter();
+
+ flatbuffers::Offset<ShooterStatus> RunIteration(
+ const ShooterGoal *goal, const ShooterPosition *position,
+ flatbuffers::FlatBufferBuilder *fbb, OutputT *output);
+
+ private:
+ FlywheelController finisher_, accelerator_left_, accelerator_right_;
+
+ DISALLOW_COPY_AND_ASSIGN(Shooter);
+};
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+
+#endif // Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index bade51e..f26b076 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -5,10 +5,10 @@
table ShooterGoal {
// Angular velocity in rad/s of the slowest (lowest) wheel in the kicker.
// Positive is shooting the ball.
- velocity_kicker:double;
+ velocity_accelerator:double;
// Angular velocity in rad/s of the flywheel. Positive is shooting.
- velocity_flywheel:double;
+ velocity_finisher:double;
}
table Goal {
diff --git a/y2020/control_loops/superstructure/superstructure_output.fbs b/y2020/control_loops/superstructure/superstructure_output.fbs
index c14c08f..2583106 100644
--- a/y2020/control_loops/superstructure/superstructure_output.fbs
+++ b/y2020/control_loops/superstructure/superstructure_output.fbs
@@ -23,11 +23,11 @@
washing_machine_spinner_voltage:double;
// Voltage sent to the kicker. Positive is shooting.
- kicker_left_voltage:double;
- kicker_right_voltage:double;
+ accelerator_left_voltage:double;
+ accelerator_right_voltage:double;
// Voltage sent to the flywheel. Positive is shooting.
- flywheel_voltage:double;
+ finisher_voltage:double;
// Voltage sent to the motor driving the control panel. Positive is counterclockwise from above.
control_panel_voltage:double;
diff --git a/y2020/control_loops/superstructure/superstructure_position.fbs b/y2020/control_loops/superstructure/superstructure_position.fbs
index 63f91c7..ed4ba18 100644
--- a/y2020/control_loops/superstructure/superstructure_position.fbs
+++ b/y2020/control_loops/superstructure/superstructure_position.fbs
@@ -4,12 +4,12 @@
table ShooterPosition {
// Flywheel angle in radians, positive is shooting.
- theta_flywheel:double;
+ theta_finisher:double;
// Kicker angle in radians of the slowest (lowest) wheel, positive is
// accelerating the ball toward the shooter.
- theta_kicker_left:double;
- theta_kicker_right:double;
+ theta_accelerator_left:double;
+ theta_accelerator_right:double;
}
table Position {
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 3dd2f2c..023a242 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -3,7 +3,7 @@
namespace y2020.control_loops.superstructure;
-table ShooterSegmentStatus {
+table FlywheelControllerStatus {
// The current average velocity in radians/second over the last kHistoryLength
// in shooter.h
avg_angular_velocity:double;
@@ -18,12 +18,12 @@
table ShooterStatus {
// The final wheel shooting the ball
- flywheel:ShooterSegmentStatus;
+ finisher:FlywheelControllerStatus;
- // The subsystem to accelerate the ball before the flywheel
+ // The subsystem to accelerate the ball before the finisher
// Velocity is the slowest (lowest) wheel
- kicker_left:ShooterSegmentStatus;
- kicker_right:ShooterSegmentStatus;
+ accelerator_left:FlywheelControllerStatus;
+ accelerator_right:FlywheelControllerStatus;
}
table Status {