Merge branch 'shooter_loop' into merged_loops
Conflicts:
frc971/atom_code/atom_code.gyp
diff --git a/frc971/atom_code/.gitignore b/frc971/atom_code/.gitignore
new file mode 100644
index 0000000..b1eea9c
--- /dev/null
+++ b/frc971/atom_code/.gitignore
@@ -0,0 +1,2 @@
+/shooter.csv
+/wrist.csv
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 2e3d5bc..e9c787e 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -13,6 +13,8 @@
'../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust',
'../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_lib_test',
'../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_csv',
+ '../control_loops/shooter/shooter.gyp:shooter_lib_test',
+ '../control_loops/shooter/shooter.gyp:shooter',
'../input/input.gyp:JoystickReader',
'../input/input.gyp:SensorReader',
'../input/input.gyp:GyroReader',
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
new file mode 100755
index 0000000..9cda41c
--- /dev/null
+++ b/frc971/control_loops/python/shooter.py
@@ -0,0 +1,140 @@
+#!/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 - 2700.0
+ # Free Current in Amps
+ self.free_current = 1.4
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.0012
+ # 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([.6, .981])
+
+ 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_data = numpy.genfromtxt('shooter/shooter_data.csv', delimiter=',')
+ shooter = Shooter()
+ simulated_x = []
+ real_x = []
+ x_vel = []
+ initial_x = shooter_data[0, 2]
+ last_x = initial_x
+ for i in xrange(shooter_data.shape[0]):
+ shooter.Update(numpy.matrix([[shooter_data[i, 1]]]))
+ simulated_x.append(shooter.X[0, 0])
+ x_offset = shooter_data[i, 2] - initial_x
+ real_x.append(x_offset / 2)
+ x_vel.append(shooter_data[i, 2] - last_x)
+ last_x = shooter_data[i, 2]
+
+ sim_delay = 1
+ pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
+ simulated_x, label='Simulation')
+ pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
+ pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
+ pylab.legend()
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ shooter = Shooter()
+ close_loop_x = []
+ close_loop_U = []
+ velocity_goal = 300
+ 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] = numpy.clip(R[0, 0], min_reference, max_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([[50.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 .h file name and .cc file name"
+ else:
+ if argv[1][-3:] == '.cc':
+ print '.cc file is second'
+ 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/shooter.cc b/frc971/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..cecf10f
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -0,0 +1,109 @@
+#include "frc971/control_loops/shooter/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/shooter_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+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),
+ last_position_(0.0) {
+ memset(history_, 0, sizeof(history_));
+}
+
+/*static*/ const double ShooterMotor::dt = 0.01;
+/*static*/ const double ShooterMotor::kMaxSpeed =
+ 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
+
+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) {
+ const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ const double current_position =
+ (position == NULL ? loop_->X_hat[0] : position->position);
+ double output_voltage = 0.0;
+
+ // Track the current position if the velocity goal is small.
+ if (velocity_goal <= 1.0) {
+ position_goal_ = current_position;
+ }
+
+ loop_->Y << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ // Prevents integral windup by limiting the position error such that the
+ // error can't produce much more than full power.
+ const double kVelocityWeightScalar = 0.35;
+ const double max_reference =
+ (loop_->plant.U_max[0] - kVelocityWeightScalar *
+ (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] - kVelocityWeightScalar *
+ (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(position, output == NULL);
+
+ // Kill power at low velocity goals.
+ if (velocity_goal < 1.0) {
+ loop_->U[0] = 0.0;
+ } else {
+ output_voltage = loop_->U[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, current_position,
+ current_position / (2 * M_PI),
+ (current_position - last_position_) / dt,
+ loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
+
+ // Calculates the velocity over the last kHistoryLength * .01 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_;
+
+ // Determine 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);
+
+ last_position_ = current_position;
+
+ if (output) {
+ output->voltage = output_voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
new file mode 100644
index 0000000..bf1cd67
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -0,0 +1,83 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'shooter_loop',
+ 'type': 'static_library',
+ 'sources': ['shooter_motor.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/shooter',
+ },
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ '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',
+ 'shooter_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'shooter_loop',
+ 'shooter_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_csv',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_csv.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'shooter_lib',
+ 'shooter_loop',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..77c605b
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.h
@@ -0,0 +1,54 @@
+#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/shooter_motor.q.h"
+#include "frc971/control_loops/shooter/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);
+
+ // Control loop time step.
+ static const double dt;
+
+ // Maximum speed of the shooter wheel which the encoder is rated for in
+ // rad/sec.
+ static const double kMaxSpeed;
+
+ 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:
+ // 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 = 5;
+ double history_[kHistoryLength];
+ ptrdiff_t history_position_;
+ double average_velocity_;
+
+ double position_goal_;
+ double last_position_;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_H_
diff --git a/frc971/control_loops/shooter/shooter_csv.cc b/frc971/control_loops/shooter/shooter_csv.cc
new file mode 100644
index 0000000..de2b4ee
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_csv.cc
@@ -0,0 +1,51 @@
+#include "stdio.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+
+using ::frc971::control_loops::shooter;
+using ::aos::time::Time;
+
+int main(int argc, char * argv[]) {
+ FILE *data_file = NULL;
+ FILE *output_file = NULL;
+
+ if (argc == 2) {
+ data_file = fopen(argv[1], "w");
+ output_file = data_file;
+ } else {
+ printf("Logging to stdout instead\n");
+ output_file = stdout;
+ }
+
+ fprintf(data_file, "time, power, position");
+
+ ::aos::Init();
+
+ Time start_time = Time::Now();
+
+ while (true) {
+ ::aos::time::PhasedLoop10MS(2000);
+ shooter.goal.FetchLatest();
+ shooter.status.FetchLatest();
+ shooter.position.FetchLatest();
+ shooter.output.FetchLatest();
+ if (shooter.output.get() &&
+ shooter.position.get()) {
+ fprintf(output_file, "\n%f, %f, %f",
+ (shooter.position->sent_time - start_time).ToSeconds(),
+ shooter.output->voltage,
+ shooter.position->position);
+ }
+ }
+
+ if (data_file) {
+ fclose(data_file);
+ }
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/control_loops/shooter/shooter_data.csv b/frc971/control_loops/shooter/shooter_data.csv
new file mode 100644
index 0000000..1c110f8
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_data.csv
@@ -0,0 +1,117 @@
+738.970696729,0.000000,2609.485398
+738.990725254,0.000000,2609.485398
+739.10668572,0.000000,2609.485398
+739.30868416,0.000000,2609.485398
+739.50644324,0.000000,2609.485398
+739.70993003,0.000000,2609.485398
+739.90893299,0.000000,2609.485398
+739.110790660,0.000000,2609.485398
+739.130856480,0.000000,2609.485398
+739.150843939,0.000000,2609.485398
+739.170774964,0.000000,2609.485398
+739.190835406,0.000000,2609.485398
+739.210871962,0.000000,2609.485398
+739.230838889,0.000000,2609.485398
+739.250849394,0.000000,2609.485398
+739.270710020,0.000000,2609.485398
+739.290835485,0.000000,2609.485398
+739.310975196,0.000000,2609.485398
+739.330836940,0.000000,2609.485398
+739.350841719,0.000000,2609.485398
+739.370882116,0.000000,2609.485398
+739.390924188,0.000000,2609.485398
+739.410957743,0.000000,2609.485398
+739.430902249,0.000000,2609.485398
+739.450846964,0.000000,2609.485398
+739.470722325,0.000000,2609.485398
+739.490794153,0.000000,2609.485398
+739.510974652,0.000000,2609.485398
+739.530838142,0.000000,2609.485398
+739.550941466,0.000000,2609.485398
+739.571202981,0.000000,2609.485398
+739.590857785,0.000000,2609.485398
+739.610997565,0.000000,2609.485398
+739.630899678,0.000000,2609.485398
+739.650656101,0.000000,2609.485398
+739.671141736,0.000000,2609.485398
+739.690876859,0.000000,2609.485398
+739.711039759,0.000000,2609.485398
+739.730890047,0.000000,2609.485398
+739.750892591,0.000000,2609.485398
+739.770626663,0.000000,2609.485398
+739.790778110,6.000000,2609.485398
+739.810913701,6.000000,2609.528710
+739.830692404,6.000000,2609.788585
+739.850725399,6.000000,2610.329990
+739.870711739,6.000000,2611.087957
+739.890776372,6.000000,2612.019173
+739.910893176,6.000000,2613.231920
+739.930686684,6.000000,2614.639573
+739.950843716,6.000000,2616.198820
+739.970639251,6.000000,2617.996284
+739.990663653,6.000000,2619.966998
+740.10690431,6.000000,2622.067650
+740.30636963,6.000000,2624.341551
+740.50667023,6.000000,2626.788701
+740.70669008,6.000000,2629.387445
+740.90640333,6.000000,2632.116126
+740.110875518,6.000000,2634.974745
+740.130666584,6.000000,2637.963300
+740.150771653,6.000000,2641.060137
+740.170676698,6.000000,2644.265254
+740.190627279,6.000000,2647.600309
+740.210613899,6.000000,2651.021988
+740.230642424,6.000000,2654.551949
+740.251043972,6.000000,2658.168534
+740.270911233,6.000000,2661.871744
+740.290883116,6.000000,2665.661579
+740.310902352,6.000000,2669.538039
+740.330648299,6.000000,2673.479467
+740.350654054,6.000000,2677.485864
+740.370725670,6.000000,2681.578886
+740.390879841,6.000000,2685.715220
+740.410951525,6.000000,2689.916522
+740.430850006,6.000000,2694.182794
+740.450856740,6.000000,2698.514033
+740.470878143,6.000000,2702.888586
+740.490685270,6.000000,2707.306450
+740.510980587,6.000000,2711.810940
+740.530967487,6.000000,2716.315429
+740.550840964,6.000000,2720.884887
+740.570872352,6.000000,2725.497658
+740.590882927,6.000000,2730.153741
+740.611002875,6.000000,2734.831480
+740.630612282,6.000000,2739.574187
+740.650721402,6.000000,2744.338551
+740.670723318,6.000000,2749.146228
+740.690835093,6.000000,2753.975560
+740.710883940,6.000000,2758.826549
+740.730851843,6.000000,2763.720850
+740.750864165,6.000000,2768.658463
+740.770859165,6.000000,2773.617733
+740.790917442,6.000000,2778.577002
+740.810989130,6.000000,2783.579585
+740.830854016,6.000000,2788.625479
+740.850606738,6.000000,2793.649717
+740.870962746,6.000000,2798.738924
+740.890848306,6.000000,2803.828131
+740.910991649,6.000000,2808.938994
+740.930855839,6.000000,2814.071513
+740.950973968,6.000000,2819.247345
+740.970789060,6.000000,2824.401520
+740.990920668,6.000000,2829.599008
+741.10980832,6.000000,2834.796496
+741.30662594,6.000000,2840.015640
+741.50930045,6.000000,2845.256440
+741.70977009,6.000000,2850.453928
+741.90974176,6.000000,2855.759697
+741.110959398,6.000000,2861.022153
+741.130866259,6.000000,2866.306266
+741.150815025,6.000000,2871.612035
+741.170950337,6.000000,2876.917804
+741.190887999,6.000000,2882.245229
+741.210981335,6.000000,2887.572654
+741.230875206,6.000000,2892.900079
+741.250802599,6.000000,2898.270816
+741.270604281,6.000000,2903.619898
+741.290897085,6.000000,2908.990635
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..beb62f1
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,193 @@
+#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/shooter_motor.q.h"
+#include "frc971/control_loops/shooter/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") {
+ }
+
+ // 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;
+ shooter_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+ private:
+ ShooterLoop my_shooter_loop_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ 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);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+
+ // 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);
+ }
+
+ // Bring up and down Core.
+ ::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_;
+};
+
+// 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();
+ my_shooter_loop_.output.FetchLatest();
+ EXPECT_EQ(my_shooter_loop_.output->voltage, 0.0);
+}
+
+// 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();
+ for (int i = 0; i < 100; ++i) {
+ 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_GT(shooter_motor_.kMaxSpeed,
+ shooter_motor_plant_.shooter_plant_->X(1, 0));
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+TEST_F(ShooterTest, Disabled) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(i > 50);
+ }
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/shooter/shooter_main.cc b/frc971/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..72b820e
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/shooter/shooter.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ShooterMotor shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/shooter/shooter_motor.q b/frc971/control_loops/shooter/shooter_motor.q
new file mode 100644
index 0000000..f2abf25
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor.q
@@ -0,0 +1,31 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group ShooterLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Goal velocity in rad/sec
+ double velocity;
+ };
+
+ message Status {
+ // True if the shooter is up to speed.
+ bool ready;
+ // The average velocity over the last 0.1 seconds.
+ double average_velocity;
+ };
+
+ message Position {
+ // The angle of the shooter wheel measured in rad/sec.
+ 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/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
new file mode 100644
index 0000000..c13ce2d
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/shooter/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 << 0.486400730028, 0.247515916371;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeShooterPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
new file mode 100644
index 0000000..ac42b0b
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_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_SHOOTER_MOTOR_PLANT_H_