Added shooter loop.
Still needs to actually be tested and tuned on a robot. Passes all of its tests.
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 18df792..788deff 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -2,6 +2,7 @@
'variables': {
'loop_files': [
'DriveTrain.q',
+ 'shooter_motor.q',
]
},
'targets': [
@@ -37,6 +38,48 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'shooter_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shooter.cc',
+ 'shooter_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'control_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ {
+ 'target_name': 'shooter_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'control_loops',
+ 'shooter_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ {
+ 'target_name': 'shooter',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'shooter_lib',
+ 'control_loops',
+ ],
+ },
+ {
'target_name': 'DriveTrain',
'type': 'executable',
'sources': [
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
new file mode 100755
index 0000000..ab7b163
--- /dev/null
+++ b/frc971/control_loops/python/shooter.py
@@ -0,0 +1,119 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from matplotlib import pylab
+import control_loop
+
+class Shooter(control_loop.ControlLoop):
+ def __init__(self):
+ super(Shooter, self).__init__("Shooter")
+ # Stall Torque in N m
+ self.stall_torque = 0.49819248
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.4
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.00161906
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current / 2
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 11.0 / 34.0
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.2, .15])
+
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ shooter = Shooter()
+ simulated_x = []
+ for _ in xrange(500):
+ shooter.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(shooter.X[0, 0])
+
+# pylab.plot(range(500), simulated_x)
+# pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ shooter = Shooter()
+ close_loop_x = []
+ close_loop_U = []
+ velocity_goal = 1050.0
+ R = numpy.matrix([[0.0], [velocity_goal]])
+ for _ in pylab.linspace(0,1.99,200):
+ # Iterate the position up.
+ R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
+ # Prevents the position goal from going beyond what is necessary.
+ velocity_weight_scalar = 0.35
+ max_reference = ((shooter.U_max[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) / shooter.K[0, 0]
+ + shooter.X_hat[0, 0])
+ min_reference = ((shooter.U_min[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) / shooter.K[0, 0]
+ + shooter.X_hat[0, 0])
+ R[0, 0] = max(min(R[0, 0], max_reference), min_reference)
+ U = numpy.clip(shooter.K * (R - shooter.X_hat), shooter.U_min, shooter.U_max)
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ close_loop_x.append(shooter.X[1, 0])
+ close_loop_U.append(U[0, 0])
+
+# pylab.plotfile("shooter.csv", (0,1))
+# pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
+# pylab.plotfile("shooter.csv", (0,2))
+ pylab.plot(pylab.linspace(0,1.99,200), close_loop_x, 'ro')
+ pylab.show()
+
+ # Simulate spin down.
+ spin_down_x = [];
+ R = numpy.matrix([[0.0], [0.0]])
+ for _ in xrange(150):
+ U = 0
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ spin_down_x.append(shooter.X[1, 0])
+
+# pylab.plot(range(150), spin_down_x)
+# pylab.show()
+
+
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ shooter.DumpHeaderFile(argv[1])
+ shooter.DumpCppFile(argv[2], argv[1])
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/shooter.cc b/frc971/control_loops/shooter.cc
new file mode 100644
index 0000000..102032e
--- /dev/null
+++ b/frc971/control_loops/shooter.cc
@@ -0,0 +1,143 @@
+#include "frc971/control_loops/shooter.h"
+
+#include "aos/aos_core.h"
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/shooter_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+namespace {
+// Motor controllers have a range of PWM values where they don't trigger,
+// and we use these values to avoid that.
+// TODO: find these values for the Talons.
+static const double positive_deadband_power = 0.0;
+static const double negative_deadband_power = 0.0;
+}
+
+ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
+ : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
+ loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
+ history_position_(0),
+ position_goal_(0.0),
+ time_(0.0) {
+ memset(history, 0, sizeof(history));
+ // Creates the header for the data file. Overwrites anything already there.
+ loop_->StartDataFile("shooter.csv");
+}
+
+void ShooterMotor::RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Status *status) {
+
+ bool bad_pos = false;
+ if (position == NULL) {
+ LOG(WARNING, "no position given\n");
+ bad_pos = true;
+ }
+
+ const double velocity_goal = std::min(goal->velocity, max_speed);
+ static double last_position = 0.0;
+ double output_voltage = 0.0;
+
+ if (!bad_pos) {
+ static bool first_time = true;
+ if (first_time) {
+ position_goal_ = position->position;
+ first_time = false;
+ }
+
+ // Add position to the history.
+ history[history_position_] = position->position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ loop_->Y << position->position;
+ // Prevents the position from going out of the bounds
+ // where the control loop will run the motor at full power.
+ const double velocity_weight_scalar = 0.35;
+ const double max_reference = (loop_->plant.U_max[0] - velocity_weight_scalar *
+ (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
+ / loop_->K[0] + loop_->X_hat[0];
+ const double min_reference = (loop_->plant.U_min[0] - velocity_weight_scalar *
+ (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
+ / loop_->K[0] + loop_->X_hat[0];
+ position_goal_ = std::max(std::min(position_goal_, max_reference), min_reference);
+ loop_->R << position_goal_, velocity_goal;
+ position_goal_ += velocity_goal * dt;
+ }
+
+ loop_->Update(!bad_pos, bad_pos || (output == NULL));
+ // There is no need to make the motors actively assist the spin-down.
+ if (velocity_goal < 1.0) {
+ output_voltage = 0.0;
+ // Also, reset the position incrementer to avoid accumulating error.
+ position_goal_ = position->position;
+ } else {
+ output_voltage = loop_->U[0] / 12.0;
+ }
+ LOG(DEBUG,
+ "PWM: %f, raw_pos: %f rotations: %f "
+ "junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
+ output_voltage, position->position,
+ position->position / (2 * M_PI),
+ (position->position - last_position) / dt,
+ loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
+
+// aos::DriverStationDisplay::Send(2, "RPS%3.0f(%3.0f) RPM%4.0f",
+// (position->position - last_position) * 100.0, goal->goal,
+// (position->position - last_position) / (2.0 * M_PI) * 6000.0);
+
+ // Calculates the velocity over the last kHistoryLength*.001 seconds
+ // by taking the difference between the current and next history positions.
+ int old_history_position = ((history_position_ == 0) ?
+ kHistoryLength : history_position_) - 1;
+ average_velocity_ = (history[old_history_position]
+ - history[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+ status->average_velocity = average_velocity_;
+ // Determines if the velocity is close enough to the goal to be ready.
+ if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
+ velocity_goal != 0.0) {
+ LOG(DEBUG, "Steady: ");
+ status->ready = true;
+ } else {
+ LOG(DEBUG, "Not ready: ");
+ status->ready = false;
+ }
+ LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
+
+ // Deal with motor controller deadbands.
+ if (output_voltage > 0) {
+ output_voltage += positive_deadband_power;
+ }
+ if (output_voltage < 0) {
+ output_voltage -= negative_deadband_power;
+ }
+
+ if (bad_pos) {
+ last_position = position->position;
+ } else {
+ // use the predicted position
+ last_position = loop_->X_hat[0];
+ }
+ if (output) {
+ output->voltage = output_voltage;
+ }
+
+ // If anything is happening, record it. Otherwise, reset the time.
+ // This should be removed once we are done with testing.
+ if (output_voltage != 0 && average_velocity_ != 0 && velocity_goal != 0) {
+ loop_->RecordDatum("shooter.csv", time_);
+ time_ += dt;
+ }
+ else {
+ time_ = 0;
+ }
+} // RunIteration
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter.h b/frc971/control_loops/shooter.h
new file mode 100644
index 0000000..f26c42d
--- /dev/null
+++ b/frc971/control_loops/shooter.h
@@ -0,0 +1,55 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/shooter_motor.q.h"
+#include "frc971/control_loops/shooter_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class ShooterMotor
+ : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ public:
+ explicit ShooterMotor(
+ control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Status *status);
+ private:
+ // Timestep of the control loop (in seconds)
+ static constexpr double dt = 0.01;
+
+ // Maximum speed of the shooter wheel which the encoder is rated for.
+ // 10000 rpm * (2 * M_PI radians / rotation) / (60 sec / min) * 15 / 34
+ static constexpr double max_speed = 461.998919646;
+
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+
+ // History array and stuff for determining average velocity and whether
+ // we are ready to shoot.
+ static const int kHistoryLength = 10;
+ double history[kHistoryLength];
+ ptrdiff_t history_position_;
+ double average_velocity_;
+
+ double position_goal_;
+
+ // time_ is used for recording data so that we have a time.
+ double time_;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_H_
diff --git a/frc971/control_loops/shooter_lib_test.cc b/frc971/control_loops/shooter_lib_test.cc
new file mode 100644
index 0000000..63afb80
--- /dev/null
+++ b/frc971/control_loops/shooter_lib_test.cc
@@ -0,0 +1,165 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/shooter_motor.q.h"
+#include "frc971/control_loops/shooter.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterMotorSimulation {
+ public:
+ // Constructs a shooter simulation. I'm not sure how the construction of
+ // the queue (my_shooter_loop_) actually works (same format as wrist).
+ ShooterMotorSimulation()
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
+ my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x78d8e372, ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status") {
+ Reinitialize();
+ }
+
+ // Resets the shooter simulation.
+ void Reinitialize() {
+ shooter_plant_->X(0, 0) = 0;
+ shooter_plant_->X(1, 0) = 0;
+ shooter_plant_->Y = shooter_plant_->C * shooter_plant_->X;
+ }
+
+ // Sends a queue message with the position of the shooter.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+ my_shooter_loop_.position.MakeMessage();
+ position->position = shooter_plant_->Y(0, 0);
+ position.Send();
+ }
+
+ // Simulates shooter for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ shooter_plant_->U << my_shooter_loop_.output->voltage * 12;
+ shooter_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+ private:
+ ShooterLoop my_shooter_loop_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ // I haven't the faintest idea exactly what this line does,
+ // but without it things break.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue "so that it invalidates the queue
+ // that it points to Otherwise, we will have a pointed to
+ // shared memory that is no longer valid."
+ ShooterLoop my_shooter_loop_;
+
+ // Create a control loop and simulation.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
+
+ ShooterTest() : my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x78d8e372,
+ ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status"),
+ shooter_motor_(&my_shooter_loop_),
+ shooter_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ // Update the robot state. Without this, the Iteration of the control loop
+ // will stop all the motors and the shooter won't go anywhere.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_NEAR(my_shooter_loop_.goal->velocity,
+ my_shooter_loop_.status->average_velocity,
+ 10.0);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ SendDSPacket(true);
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ VerifyNearGoal();
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(450.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+ VerifyNearGoal();
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+}
+
+// Test to make sure that it does not exceed the encoder's rated speed.
+TEST_F(ShooterTest, RateLimitTest) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(1000.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_FALSE(std::abs(my_shooter_loop_.goal->velocity -
+ my_shooter_loop_.status->average_velocity) < 10.0);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter_main.cc b/frc971/control_loops/shooter_main.cc
new file mode 100644
index 0000000..94b8d59
--- /dev/null
+++ b/frc971/control_loops/shooter_main.cc
@@ -0,0 +1,5 @@
+#include "frc971/control_loops/shooter.h"
+
+#include "aos/aos_core.h"
+
+AOS_RUN_LOOP(frc971::control_loops::ShooterMotor);
diff --git a/frc971/control_loops/shooter_motor.q b/frc971/control_loops/shooter_motor.q
new file mode 100644
index 0000000..29a40de
--- /dev/null
+++ b/frc971/control_loops/shooter_motor.q
@@ -0,0 +1,27 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group ShooterLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ double velocity;
+ };
+
+ message Status {
+ bool ready;
+ double average_velocity;
+ };
+
+ message Position {
+ double position;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue Status status;
+};
+
+queue_group ShooterLoop shooter;
diff --git a/frc971/control_loops/shooter_motor_plant.cc b/frc971/control_loops/shooter_motor_plant.cc
new file mode 100644
index 0000000..db11e7f
--- /dev/null
+++ b/frc971/control_loops/shooter_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/shooter_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.0098571228289, 0.0, 0.971561310859;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.00785005397639, 1.56249765488;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.07156131086, 28.0940195016;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 43.5200653183, 0.819154156845;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeShooterPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/shooter_motor_plant.h b/frc971/control_loops/shooter_motor_plant.h
new file mode 100644
index 0000000..849c869
--- /dev/null
+++ b/frc971/control_loops/shooter_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_MOTOR_PLANT_H_