Designed a shooter loop using voltage error estimation.
This gets rid of all the corner cases with the moving position style
loop from before, and doesn't have overshoot issues like standard
integral loops have.
Change-Id: I4e4eb1767038563cf851040ce8218e73ca60904a
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
index c81d597..3bb22a9 100755
--- a/y2016/control_loops/python/shooter.py
+++ b/y2016/control_loops/python/shooter.py
@@ -1,13 +1,21 @@
#!/usr/bin/python
from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
import numpy
import sys
from matplotlib import pylab
-class Shooter(control_loop.ControlLoop):
- def __init__(self):
- super(Shooter, self).__init__("Shooter")
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+class VelocityShooter(control_loop.ControlLoop):
+ def __init__(self, name='VelocityShooter'):
+ super(VelocityShooter, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.71
# Stall Current in Amps
@@ -31,43 +39,243 @@
self.dt = 0.005
# State feedback matrices
- # [position, angular velocity]
+ # [angular velocity]
self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ [[-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.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([.87])
+
+ self.PlaceObserverPoles([0.3])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+
+class Shooter(VelocityShooter):
+ def __init__(self, name='Shooter'):
+ super(Shooter, self).__init__(name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+ self.A_continuous[0, 1] = 1
+
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+ # State feedback matrices
+ # [position, angular velocity]
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- 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]])
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.K[0, 1:2] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+ self.Kff[0, 1:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
+
+class IntegralShooter(Shooter):
+ def __init__(self, name="IntegralShooter"):
+ super(IntegralShooter, self).__init__(name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 0.08
+ q_vel = 4.00
+ q_voltage = 0.2
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0],
+ [0.0, 0.0, (q_voltage ** 2.0)]])
+
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
+
+class ScenarioPlotter(object):
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
+ observer_shooter=None):
+ """Runs the shooter plant with an initial condition and goal.
+
+ Test for whether the goal has been reached and whether the separation
+ goes outside of the initial and goal values by more than
+ max_separation_error.
+
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ shooter: shooter object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_shooter: Wrist object to get K from, or None if we should
+ use shooter.
+ observer_shooter: Wrist object to use for the observer, or None if we should
+ use the actual state.
+ """
+
+ if controller_shooter is None:
+ controller_shooter = shooter
+
+ vbat = 12.0
+
+ if self.t:
+ initial_t = self.t[-1] + shooter.dt
+ else:
+ initial_t = 0
+
+ for i in xrange(iterations):
+ X_hat = shooter.X
+
+ if observer_shooter is not None:
+ X_hat = observer_shooter.X_hat
+ self.x_hat.append(observer_shooter.X_hat[1, 0])
+
+ ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
+
+ U = controller_shooter.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(shooter.X[0, 0])
+
+
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+
+ self.v.append(shooter.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / shooter.dt)
+
+ if observer_shooter is not None:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
+ self.offset.append(observer_shooter.X_hat[2, 0])
+
+ applied_U = U.copy()
+ if i > 30:
+ applied_U += 2
+ shooter.Update(applied_U)
+
+ if observer_shooter is not None:
+ observer_shooter.PredictObserver(U)
+
+ self.t.append(initial_t + i * shooter.dt)
+ self.u.append(U[0, 0])
+
+ glog.debug('Time: %f', self.t[-1])
+
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.v, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.show()
def main(argv):
- if len(argv) != 3:
- print "Expected .h file name and .cc file name"
+ scenario_plotter = ScenarioPlotter()
+
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter = IntegralShooter()
+
+ # Test moving the shooter with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter, iterations=200)
+
+ if FLAGS.plot:
+ scenario_plotter.Plot()
+
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
else:
namespaces = ['y2016', 'control_loops', 'shooter']
- shooter = Shooter()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter],
+ shooter = Shooter('Shooter')
+ loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
namespaces=namespaces)
- if argv[1][-3:] == '.cc':
- loop_writer.Write(argv[2], argv[1])
- else:
- loop_writer.Write(argv[1], argv[2])
+ loop_writer.Write(argv[1], argv[2])
+
+ integral_shooter = IntegralShooter('IntegralShooter')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralShooter', [integral_shooter], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index 2ad1d07..4d4f5f1 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -238,8 +238,6 @@
def main(argv):
- argv = FLAGS(argv)
-
scenario_plotter = ScenarioPlotter()
wrist = Wrist()
@@ -260,7 +258,7 @@
glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
else:
namespaces = ['y2016', 'control_loops', 'superstructure']
- wrist = Wrist("Wrist")
+ wrist = Wrist('Wrist')
loop_writer = control_loop.ControlLoopWriter(
'Wrist', [wrist], namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
@@ -271,4 +269,5 @@
integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))