changed to only update one plant, copied and renamed some code from last year's wrist
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
new file mode 100755
index 0000000..11699ac
--- /dev/null
+++ b/frc971/control_loops/python/shooter.py
@@ -0,0 +1,154 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Shooter(control_loop.ControlLoop):
+ def __init__(self, name="RawShooter"):
+ super(Shooter, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = .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.4
+ # Moment of inertia of the shooter in kg m^2
+ # TODO(aschuh): Measure this in reality. It doesn't seem high enough.
+ # James measured 0.51, but that can't be right given what I am seeing.
+ self.J = 2.0
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003 #TODO comment on these constants
+ # 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 / ((84.0 / 20.0) * (50.0 / 14.0) * (40.0 / 14.0) * (40.0 / 12.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.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([0.85, 0.45])
+
+ 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 ShooterDeltaU(Shooter):
+ def __init__(self, name="Shooter"):
+ super(ShooterDeltaU, 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.55, 0.35, 0.80])
+
+ 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.90])
+ 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 ClipDeltaU(shooter, delta_u):
+ old_u = numpy.matrix([[shooter.X[2, 0]]])
+ new_u = numpy.clip(old_u + delta_u, shooter.U_min, shooter.U_max)
+ return new_u - old_u
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ shooter = ShooterDeltaU()
+ simulated_x = []
+ for _ in xrange(100):
+ shooter.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(shooter.X[0, 0])
+
+ pylab.plot(range(100), simulated_x)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ shooter = ShooterDeltaU()
+ close_loop_x = []
+ close_loop_u = []
+ R = numpy.matrix([[1.0], [0.0], [0.0]])
+ shooter.X[2, 0] = -5
+ for _ in xrange(100):
+ U = numpy.clip(shooter.K * (R - shooter.X_hat), shooter.U_min, shooter.U_max)
+ U = ClipDeltaU(shooter, U)
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ close_loop_x.append(shooter.X[0, 0] * 10)
+ close_loop_u.append(shooter.X[2, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.plot(range(100), close_loop_u)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name for"
+ print "both the plant and unaugmented plant"
+ else:
+ unaug_shooter = Shooter("RawShooter")
+ unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
+ [unaug_shooter])
+ #if argv[3][-3:] == '.cc':
+ # unaug_loop_writer.Write(argv[4], argv[3])
+ #else:
+ # unaug_loop_writer.Write(argv[3], argv[4])
+
+ 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/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..3d3e7ae
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -0,0 +1,80 @@
+#include "frc971/control_loops/shooters/shooters.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/shooters/top_shooter_motor_plant.h"
+#include "frc971/control_loops/shooters/bottom_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),
+ zeroed_joint_(MakeShooterLoop()) {
+ {
+ using ::frc971::constants::GetValues;
+ ZeroedJoint<1>::ConfigurationData config_data;
+
+ config_data.lower_limit = GetValues().shooter_lower_limit;
+ config_data.upper_limit = GetValues().shooter_upper_limit;
+ config_data.hall_effect_start_angle[0] =
+ GetValues().shooter_hall_effect_start_angle;
+ config_data.zeroing_off_speed = GetValues().shooter_zeroing_off_speed;
+ config_data.zeroing_speed = GetValues().shooter_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 ShooterMotor::RunIteration(
+ const ::aos::control_loops::Goal *goal,
+ const control_loops::ShooterLoop::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/shooter/shooter_motor_plant.cc b/frc971/control_loops/shooter/shooter_motor_plant.cc
new file mode 100644
index 0000000..1b0e13c
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 3, 3> A;
+ A << 1.0, 0.00988697090637, 0.000120553991591, 0.0, 0.977479674375, 0.0240196135246, 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> MakeShooterController() {
+ Eigen::Matrix<double, 3, 1> L;
+ L << 1.97747967438, 101.419434198, 375.761249895;
+ Eigen::Matrix<double, 1, 3> K;
+ K << 243.5509628, 18.9166116502, 1.27747967438;
+ return StateFeedbackController<3, 1, 1>(L, K, MakeShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<3, 1, 1> MakeShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<3, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<3, 1, 1>(MakeShooterPlantCoefficients());
+ return StateFeedbackPlant<3, 1, 1>(plants);
+}
+
+StateFeedbackLoop<3, 1, 1> MakeShooterLoop() {
+ ::std::vector<StateFeedbackController<3, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<3, 1, 1>(MakeShooterController());
+ return StateFeedbackLoop<3, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace frc971
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..f23a38f
--- /dev/null
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -0,0 +1,20 @@
+#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 {
+
+StateFeedbackPlantCoefficients<3, 1, 1> MakeShooterPlantCoefficients();
+
+StateFeedbackController<3, 1, 1> MakeShooterController();
+
+StateFeedbackPlant<3, 1, 1> MakeShooterPlant();
+
+StateFeedbackLoop<3, 1, 1> MakeShooterLoop();
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_