started working on a skeleton for the 2014 control loops
diff --git a/frc971/control_loops/python/angle_adjust.py b/frc971/control_loops/python/angle_adjust.py
deleted file mode 100755
index 36cb49d..0000000
--- a/frc971/control_loops/python/angle_adjust.py
+++ /dev/null
@@ -1,153 +0,0 @@
-#!/usr/bin/python
-
-import control_loop
-import numpy
-import sys
-from matplotlib import pylab
-
-class AngleAdjust(control_loop.ControlLoop):
- def __init__(self, name="AngleAdjustRaw"):
- super(AngleAdjust, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = .428
- # Stall Current in Amps
- self.stall_current = 63.8
- # Free Speed in RPM
- self.free_speed = 14900.0
- # Free Current in Amps
- self.free_current = 1.2
- # Moment of inertia of the angle adjust about the shooter's pivot in kg m^2
- self.J = 9.4
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # 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 of the gearbox multiplied by the ratio of the radii of
- # the output and the angle adjust curve, which is essentially another gear.
- self.G = (1.0 / 50.0) * (0.01905 / 0.41964)
- # 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.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
-
- self.PlaceControllerPoles([.45, .83])
-
- print "Unaugmented controller poles at"
- print self.K
-
- self.rpl = .05
- self.ipl = 0.008
- 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]])
-
- self.InitializeState()
-
-class AngleAdjustDeltaU(AngleAdjust):
- def __init__(self, name="AngleAdjust"):
- super(AngleAdjustDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
-
- self.A = numpy.matrix([[0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0],
- [0.0, 0.0, 1.0]])
- self.A[0:2, 0:2] = A_unaugmented
- self.A[0:2, 2] = B_unaugmented
-
- self.B = numpy.matrix([[0.0],
- [0.0],
- [1.0]])
-
- self.C = numpy.matrix([[1.0, 0.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
-
- self.PlaceControllerPoles([0.60, 0.31, 0.78])
-
- print "K"
- print self.K
- print "Placed controller poles are"
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
-
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl, 0.78])
- print "Placed observer poles are"
- print numpy.linalg.eig(self.A - self.L * self.C)[0]
-
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
-
- self.InitializeState()
-
-
-def main(argv):
- # Simulate the response of the system to a step input.
- angle_adjust_data = numpy.genfromtxt(
- 'angle_adjust/angle_adjust_data.csv', delimiter=',')
- angle_adjust = AngleAdjust()
- simulated_x = []
- real_x = []
- initial_x = angle_adjust_data[0, 2]
- for i in xrange(angle_adjust_data.shape[0]):
- angle_adjust.Update(numpy.matrix([[angle_adjust_data[i, 1] - 0.7]]))
- simulated_x.append(angle_adjust.X[0, 0])
- x_offset = angle_adjust_data[i, 2] - initial_x
- real_x.append(x_offset)
-
- sim_delay = 2
- pylab.plot(range(sim_delay, angle_adjust_data.shape[0] + sim_delay),
- simulated_x, label='Simulation')
- pylab.plot(range(angle_adjust_data.shape[0]), real_x, label='Reality')
- pylab.legend()
- pylab.show()
-
- # Simulate the closed loop response of the system to a step input.
- angle_adjust = AngleAdjustDeltaU()
- close_loop_x = []
- R = numpy.matrix([[1.0], [0.0], [0.0]])
- for _ in xrange(100):
- U = numpy.clip(angle_adjust.K * (R - angle_adjust.X_hat), angle_adjust.U_min, angle_adjust.U_max)
- angle_adjust.UpdateObserver(U)
- angle_adjust.Update(U)
- close_loop_x.append(angle_adjust.X[0, 0])
-
- pylab.plot(range(100), close_loop_x)
- pylab.show()
-
- # Write the generated constants out to a file.
- if len(argv) != 5:
- print "Expected .cc file name and .h file name"
- else:
- loop_writer = control_loop.ControlLoopWriter("RawAngleAdjust",
- [AngleAdjust()])
- if argv[3][-3:] == '.cc':
- loop_writer.Write(argv[4], argv[3])
- else:
- loop_writer.Write(argv[3], argv[4])
-
- loop_writer = control_loop.ControlLoopWriter("AngleAdjust", [angle_adjust])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/index.py b/frc971/control_loops/python/index.py
deleted file mode 100755
index f165afe..0000000
--- a/frc971/control_loops/python/index.py
+++ /dev/null
@@ -1,104 +0,0 @@
-#!/usr/bin/python
-
-import control_loop
-import numpy
-import sys
-from matplotlib import pylab
-
-class Index(control_loop.ControlLoop):
- def __init__(self, J=0.00013, name="Index"):
- super(Index, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.4862
- # 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.5
- # Moment of inertia of the index in kg m^2
- self.J = J
- # Resistance of the motor
- self.R = 12.0 / self.stall_current + 0.024 + .003
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (13.5 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.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([.40, .63])
-
- self.rpl = .05
- self.ipl = 0.008
- 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.
- index = Index()
- simulated_x = []
- simulated_v = []
- for _ in xrange(100):
- index.Update(numpy.matrix([[12.0]]))
- simulated_x.append(index.X[0, 0])
- simulated_v.append(index.X[1, 0])
-
- pylab.plot(range(100), simulated_v)
- pylab.show()
-
- # Simulate the closed loop response of the system to a step input.
- index = Index()
- close_loop_x = []
- R = numpy.matrix([[1.0], [0.0]])
- for _ in xrange(100):
- U = numpy.clip(index.K * (R - index.X_hat), index.U_min, index.U_max)
- index.UpdateObserver(U)
- index.Update(U)
- close_loop_x.append(index.X[0, 0])
-
- pylab.plot(range(100), close_loop_x)
- pylab.show()
-
- # Set the constants for the number of discs that we expect to see.
- # The c++ code expects that the index in the array will be the number of
- # discs.
- index0 = Index(0.00010, "Index0Disc")
- index1 = Index(0.00013, "Index1Disc")
- index2 = Index(0.00013, "Index2Disc")
- index3 = Index(0.00018, "Index3Disc")
- index4 = Index(0.00025, "Index4Disc")
-
- # Write the generated constants out to a file.
- if len(argv) != 3:
- print "Expected .h file name and .c file name"
- else:
- loop_writer = control_loop.ControlLoopWriter(
- "Index", [index0, index1, index2, index3, index4])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
deleted file mode 100755
index 83beb90..0000000
--- a/frc971/control_loops/python/shooter.py
+++ /dev/null
@@ -1,140 +0,0 @@
-#!/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 - 1500.0
- # Free Current in Amps
- self.free_current = 1.4
- # Moment of inertia of the shooter wheel in kg m^2
- self.J = 0.0032
- # 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)
- x_vel.append((shooter_data[i, 2] - last_x) * 100.0)
- 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:
- loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
-
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrists.py
similarity index 100%
rename from frc971/control_loops/python/wrist.py
rename to frc971/control_loops/python/wrists.py
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
new file mode 100644
index 0000000..c4e9971
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -0,0 +1,65 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'shooter_loop',
+ 'type': 'static_library',
+ 'sources': ['shooter.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/shooter',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(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': [
+ 'shooter_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ 'shooter_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'shooter_loop',
+ 'shooter_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'shooter_lib',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
new file mode 100644
index 0000000..2b505e1
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.q
@@ -0,0 +1,33 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_looops.q";
+
+queue_group ShooterLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The energy to load to in joules.
+ double energy;
+ // Shoots as soon as this is true.
+ bool shoot;
+ };
+ message Position {
+ bool back_hall_effect;
+ // In meters, out is positive.
+ double position;
+ double back_calibration;
+ };
+ message Status {
+ // Whether it's ready to shoot immediately or not.
+ bool ready;
+ // How many times we've shot.
+ int shots;
+ };
+
+ 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_main.cc b/frc971/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..cb6fc8b
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/shooter/shooter.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ShooterLoop shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/update_shooter.sh b/frc971/control_loops/update_shooter.sh
new file mode 100755
index 0000000..8a18289
--- /dev/null
+++ b/frc971/control_loops/update_shooter.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+#
+# Updates the shooter controller.
+
+cd $(dirname $0)
+
+./python/shooter.py shooter/shooter_motor_plant.h \
+ shooter/shooter_motor_plant.cc
diff --git a/frc971/control_loops/update_wrists.sh b/frc971/control_loops/update_wrists.sh
new file mode 100755
index 0000000..bb79a0a
--- /dev/null
+++ b/frc971/control_loops/update_wrists.sh
@@ -0,0 +1,10 @@
+#!/bin/bash
+#
+# Updates the wrist controllers.
+
+cd $(dirname $0)
+
+./python/wrists.py wrists/top_wrist_motor_plant.h \
+ wrists/top_wrist_motor_plant.cc \
+ wrists/bottom_wrist_motor_plant.h \
+ wrists/bottom_wrist_motor_plant.cc
diff --git a/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc b/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc
new file mode 100644
index 0000000..f08793b
--- /dev/null
+++ b/frc971/control_loops/wrists/bottom_wrist_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00904786878843, 0.0, 0.815818233346;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.000326582411818, 0.0631746179893;
+ 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 StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeRawWristController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.71581823335, 64.8264890043;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 130.590421637, 7.48987035533;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeRawWristPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeRawWristPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeRawWristPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeRawWristLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeRawWristController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrists/bottom_wrist_motor_plant.h b/frc971/control_loops/wrists/bottom_wrist_motor_plant.h
new file mode 100644
index 0000000..00db927
--- /dev/null
+++ b/frc971/control_loops/wrists/bottom_wrist_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeRawWristPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeRawWristController();
+
+StateFeedbackPlant<2, 1, 1> MakeRawWristPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeRawWristLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRISTS_BOTTOM_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrists/top_wrist_motor_plant.cc b/frc971/control_loops/wrists/top_wrist_motor_plant.cc
new file mode 100644
index 0000000..132ef4f
--- /dev/null
+++ b/frc971/control_loops/wrists/top_wrist_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00904786878843, 0.000326582411818, 0.0, 0.815818233346, 0.0631746179893, 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 3, 1> B;
+ B << 0.0, 0.0, 1.0;
+ Eigen::Matrix<double, 1, 3> C;
+ C << 1.0, 0.0, 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0.0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<3, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<3, 1, 1> MakeWristController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 1.81581823335, 78.6334534274, 142.868137351;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 92.6004807973, 4.38063492858, 1.11581823335;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeWristPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeWristPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeWristPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeWristLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeWristController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrists/top_wrist_motor_plant.h b/frc971/control_loops/wrists/top_wrist_motor_plant.h
new file mode 100644
index 0000000..ac657cb
--- /dev/null
+++ b/frc971/control_loops/wrists/top_wrist_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeWristPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeWristController();
+
+StateFeedbackPlant<3, 1, 1> MakeWristPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeWristLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRISTS_TOP_WRIST_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/wrists/wrists.cc b/frc971/control_loops/wrists/wrists.cc
new file mode 100644
index 0000000..fe9295b
--- /dev/null
+++ b/frc971/control_loops/wrists/wrists.cc
@@ -0,0 +1,80 @@
+#include "frc971/control_loops/wrists/wrists.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
+#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
+ : aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
+ zeroed_joint_(MakeWristLoop()) {
+ {
+ using ::frc971::constants::GetValues;
+ ZeroedJoint<1>::ConfigurationData config_data;
+
+ config_data.lower_limit = GetValues().wrist_lower_limit;
+ config_data.upper_limit = GetValues().wrist_upper_limit;
+ config_data.hall_effect_start_angle[0] =
+ GetValues().wrist_hall_effect_start_angle;
+ config_data.zeroing_off_speed = GetValues().wrist_zeroing_off_speed;
+ config_data.zeroing_speed = GetValues().wrist_zeroing_speed;
+
+ config_data.max_zeroing_voltage = 5.0;
+ config_data.deadband_voltage = 0.0;
+
+ zeroed_joint_.set_config_data(config_data);
+ }
+}
+
+// Positive angle is up, and positive power is up.
+void WristMotor::RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::WristLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status * status) {
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) {
+ output->voltage = 0;
+ }
+
+ ZeroedJoint<1>::PositionData transformed_position;
+ ZeroedJoint<1>::PositionData *transformed_position_ptr =
+ &transformed_position;
+ if (!position) {
+ transformed_position_ptr = NULL;
+ } else {
+ transformed_position.position = position->pos;
+ transformed_position.hall_effects[0] = position->hall_effect;
+ transformed_position.hall_effect_positions[0] = position->calibration;
+ }
+
+ const double voltage = zeroed_joint_.Update(transformed_position_ptr,
+ output != NULL,
+ goal->goal, 0.0);
+
+ if (position) {
+ LOG(DEBUG, "pos: %f hall: %s absolute: %f\n",
+ position->pos,
+ position->hall_effect ? "true" : "false",
+ zeroed_joint_.absolute_position());
+ }
+
+ if (output) {
+ output->voltage = voltage;
+ }
+ status->done = ::std::abs(zeroed_joint_.absolute_position() - goal->goal) < 0.004;
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrists/wrists.gyp b/frc971/control_loops/wrists/wrists.gyp
new file mode 100644
index 0000000..f455e2e
--- /dev/null
+++ b/frc971/control_loops/wrists/wrists.gyp
@@ -0,0 +1,66 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'wrist_loops',
+ 'type': 'static_library',
+ 'sources': ['wrists.q'],
+ 'variables': {
+ 'header_path': 'frc971/control_loops/wrists',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'wrists_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'wrists.cc',
+ 'top_wrist_motor_plant.cc',
+ 'top_wrist_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'wrist_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ 'export_dependent_settings': [
+ 'wrist_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'wrists_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'wrists_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'wrist_loops',
+ 'wrists_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'wrists',
+ 'type': 'executable',
+ 'sources': [
+ 'wrists_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'wrists_lib',
+ ],
+ },
+ ],
+}
diff --git a/frc971/control_loops/wrists/wrists.h b/frc971/control_loops/wrists/wrists.h
new file mode 100644
index 0000000..1c1fdf4
--- /dev/null
+++ b/frc971/control_loops/wrists/wrists.h
@@ -0,0 +1,63 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
+#define FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/wrists/wrists.q.h"
+#include "frc971/control_loops/wrists/top_wrist_motor_plant.h"
+#include "frc971/control_loops/wrists/bottom_wrist_motor_plant.h"
+
+#include "frc971/control_loops/zeroed_joint.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class WristTest_NoWindupPositive_Test;
+class WristTest_NoWindupNegative_Test;
+};
+
+class WristMotor
+ : public aos::control_loops::ControlLoop<control_loops::WristLoop> {
+ public:
+ explicit WristMotor(
+ control_loops::WristLoop *my_wrist = &control_loops::wrist);
+
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return zeroed_joint_.capped_goal(); }
+
+ // True if the wrist is zeroing.
+ bool is_zeroing() const { return zeroed_joint_.is_zeroing(); }
+
+ // True if the wrist is zeroing.
+ bool is_moving_off() const { return zeroed_joint_.is_moving_off(); }
+
+ // True if the state machine is uninitialized.
+ bool is_uninitialized() const { return zeroed_joint_.is_uninitialized(); }
+
+ // True if the state machine is ready.
+ bool is_ready() const { return zeroed_joint_.is_ready(); }
+
+ protected:
+ virtual void RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::WristLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ ::aos::control_loops::Status *status);
+
+ private:
+ // Friend the test classes for acces to the internal state.
+ friend class testing::WristTest_NoWindupPositive_Test;
+ friend class testing::WristTest_NoWindupNegative_Test;
+
+ // The zeroed joint to use.
+ ZeroedJoint<1> zeroed_joint_;
+
+ DISALLOW_COPY_AND_ASSIGN(WristMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_WRIST_H_
diff --git a/frc971/control_loops/wrists/wrists.q b/frc971/control_loops/wrists/wrists.q
new file mode 100644
index 0000000..0f5a4a8
--- /dev/null
+++ b/frc971/control_loops/wrists/wrists.q
@@ -0,0 +1,28 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+// All angles here are 0 horizontal, positive up.
+queue_group WristsLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The angle of the bottom wrist.
+ double bottom_angle;
+ // How much higher the top wrist is.
+ double between_angle;
+ bool intake;
+ };
+ message Position {
+ double bottom_position, top_position;
+ bool bottom_hall_effect, top_hall_effect;
+ double bottom_calibration, top_calibration;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue aos.control_loops.Status status;
+};
+
+queue_group WristsLoop wrists;
diff --git a/frc971/control_loops/wrists/wrists_lib_test.cc b/frc971/control_loops/wrists/wrists_lib_test.cc
new file mode 100644
index 0000000..996036e
--- /dev/null
+++ b/frc971/control_loops/wrists/wrists_lib_test.cc
@@ -0,0 +1,334 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/wrists/wrists.q.h"
+#include "frc971/control_loops/wrists/wrists.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the wrist and sends out queue messages containing the
+// position.
+class WristMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // wrist, which will be treated as 0 by the encoder.
+ WristMotorSimulation(double initial_position)
+ : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawWristPlant())),
+ my_wrist_loop_(".frc971.control_loops.wrists",
+ 0x1a7b7094, ".frc971.control_loops.wrists.goal",
+ ".frc971.control_loops.wrists.position",
+ ".frc971.control_loops.wrists.output",
+ ".frc971.control_loops.wrists.status") {
+ Reinitialize(initial_position);
+ }
+
+ // Resets the plant so that it starts at initial_position.
+ void Reinitialize(double initial_position) {
+ initial_position_ = initial_position;
+ wrist_plant_->X(0, 0) = initial_position_;
+ wrist_plant_->X(1, 0) = 0.0;
+ wrist_plant_->Y = wrist_plant_->C() * wrist_plant_->X;
+ last_position_ = wrist_plant_->Y(0, 0);
+ calibration_value_ = 0.0;
+ last_voltage_ = 0.0;
+ }
+
+ // Returns the absolute angle of the wrist.
+ double GetAbsolutePosition() const {
+ return wrist_plant_->Y(0, 0);
+ }
+
+ // Returns the adjusted angle of the wrist.
+ double GetPosition() const {
+ return GetAbsolutePosition() - initial_position_;
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ const double angle = GetPosition();
+
+ ::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
+ my_wrist_loop_.position.MakeMessage();
+ position->pos = angle;
+
+ // Signal that the hall effect sensor has been triggered if it is within
+ // the correct range.
+ double abs_position = GetAbsolutePosition();
+ if (abs_position >= constants::GetValues().wrist_hall_effect_start_angle &&
+ abs_position <= constants::GetValues().wrist_hall_effect_stop_angle) {
+ position->hall_effect = true;
+ } else {
+ position->hall_effect = false;
+ }
+
+ // Only set calibration if it changed last cycle. Calibration starts out
+ // with a value of 0.
+ if ((last_position_ <
+ constants::GetValues().wrist_hall_effect_start_angle ||
+ last_position_ >
+ constants::GetValues().wrist_hall_effect_stop_angle) &&
+ position->hall_effect) {
+ calibration_value_ =
+ constants::GetValues().wrist_hall_effect_start_angle -
+ initial_position_;
+ }
+
+ position->calibration = calibration_value_;
+ position.Send();
+ }
+
+ // Simulates the wrist moving for one timestep.
+ void Simulate() {
+ last_position_ = wrist_plant_->Y(0, 0);
+ EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
+ wrist_plant_->U << last_voltage_;
+ wrist_plant_->Update();
+
+ EXPECT_GE(constants::GetValues().wrist_upper_physical_limit,
+ wrist_plant_->Y(0, 0));
+ EXPECT_LE(constants::GetValues().wrist_lower_physical_limit,
+ wrist_plant_->Y(0, 0));
+ last_voltage_ = my_wrist_loop_.output->voltage;
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
+ private:
+ WristLoop my_wrist_loop_;
+ double initial_position_;
+ double last_position_;
+ double calibration_value_;
+ double last_voltage_;
+};
+
+class WristTest : public ::testing::Test {
+ protected:
+ ::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 pointer to shared memory that
+ // is no longer valid.
+ WristLoop my_wrist_loop_;
+
+ // Create a loop and simulation plant.
+ WristMotor wrist_motor_;
+ WristMotorSimulation wrist_motor_plant_;
+
+ WristTest() : my_wrist_loop_(".frc971.control_loops.wrist",
+ 0x1a7b7094, ".frc971.control_loops.wrist.goal",
+ ".frc971.control_loops.wrist.position",
+ ".frc971.control_loops.wrist.output",
+ ".frc971.control_loops.wrist.status"),
+ wrist_motor_(&my_wrist_loop_),
+ wrist_motor_plant_(0.5) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_wrist_loop_.goal.FetchLatest();
+ my_wrist_loop_.position.FetchLatest();
+ EXPECT_NEAR(my_wrist_loop_.goal->goal,
+ wrist_motor_plant_.GetAbsolutePosition(),
+ 1e-4);
+ }
+
+ virtual ~WristTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the wrist zeros correctly and goes to a position.
+TEST_F(WristTest, ZerosCorrectly) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the wrist zeros correctly starting on the hall effect sensor.
+TEST_F(WristTest, ZerosStartingOn) {
+ wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);
+
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that missing positions are correctly handled.
+TEST_F(WristTest, HandleMissingPosition) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 400; ++i) {
+ if (i % 23) {
+ wrist_motor_plant_.SendPositionMessage();
+ }
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that loosing the encoder for a second triggers a re-zero.
+TEST_F(WristTest, RezeroWithMissingPos) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ // After 3 seconds, simulate the encoder going missing.
+ // This should trigger a re-zero. To make sure it works, change the goal as
+ // well.
+ if (i < 300 || i > 400) {
+ wrist_motor_plant_.SendPositionMessage();
+ } else {
+ if (i > 310) {
+ // Should be re-zeroing now.
+ EXPECT_TRUE(wrist_motor_.is_uninitialized());
+ }
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
+ }
+ if (i == 410) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that disabling while zeroing sends the state machine into the
+// uninitialized state.
+TEST_F(WristTest, DisableGoesUninitialized) {
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 800; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ // After 0.5 seconds, disable the robot.
+ if (i > 50 && i < 200) {
+ SendDSPacket(false);
+ if (i > 100) {
+ // Give the loop a couple cycled to get the message and then verify that
+ // it is in the correct state.
+ EXPECT_TRUE(wrist_motor_.is_uninitialized());
+ // When disabled, we should be applying 0 power.
+ EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
+ EXPECT_EQ(0, my_wrist_loop_.output->voltage);
+ }
+ } else {
+ SendDSPacket(true);
+ }
+ if (i == 202) {
+ // Verify that we are zeroing after the bot gets enabled again.
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(WristTest, NoWindupNegative) {
+ int capped_count = 0;
+ double saved_zeroing_position = 0;
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ if (i == 50) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = -100.0;
+ } else if (i == 51) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position,
+ wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ }
+ if (!wrist_motor_.is_ready()) {
+ if (wrist_motor_.capped_goal()) {
+ ++capped_count;
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ ASSERT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+ EXPECT_GT(3, capped_count);
+}
+
+// Tests that the wrist can't get too far away from the zeroing position if the
+// zeroing position is saturating the goal.
+TEST_F(WristTest, NoWindupPositive) {
+ int capped_count = 0;
+ double saved_zeroing_position = 0;
+ my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ for (int i = 0; i < 500; ++i) {
+ wrist_motor_plant_.SendPositionMessage();
+ if (i == 50) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ // Move the zeroing position far away and verify that it gets moved back.
+ saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
+ wrist_motor_.zeroed_joint_.zeroing_position_ = 100.0;
+ } else {
+ if (i == 51) {
+ EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ }
+ }
+ if (!wrist_motor_.is_ready()) {
+ if (wrist_motor_.capped_goal()) {
+ ++capped_count;
+ // The cycle after we kick the zero position is the only cycle during
+ // which we should expect to see a high uncapped power during zeroing.
+ EXPECT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ EXPECT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+
+ wrist_motor_.Iterate();
+ wrist_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+ EXPECT_GT(3, capped_count);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/wrists/wrists_main.cc b/frc971/control_loops/wrists/wrists_main.cc
new file mode 100644
index 0000000..66d1ea3
--- /dev/null
+++ b/frc971/control_loops/wrists/wrists_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/wrist/wrists.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::WristsLoop wrists;
+ wrists.Run();
+ ::aos::Cleanup();
+ return 0;
+}