Updated the shooter loop to simplify it and add more tests. Fixed the poles to be close to last year.
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index ab7b163..663a6b2 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -9,11 +9,11 @@
def __init__(self):
super(Shooter, self).__init__("Shooter")
# Stall Torque in N m
- self.stall_torque = 0.49819248
+ self.stall_torque = 0.49819248
# Stall Current in Amps
- self.stall_current = 85
+ self.stall_current = 85
# Free Speed in RPM
- self.free_speed = 19300.0
+ 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
@@ -21,7 +21,7 @@
# 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) /
+ 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
@@ -31,19 +31,19 @@
self.dt = 0.01
# State feedback matrices
- self.A_continuous = numpy.matrix(
+ 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(
+ 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.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
self.dt, self.C)
- self.PlaceControllerPoles([.2, .15])
+ self.PlaceControllerPoles([.6, .981])
self.rpl = .45
self.ipl = 0.07
@@ -69,50 +69,57 @@
shooter = Shooter()
close_loop_x = []
close_loop_U = []
- velocity_goal = 1050.0
+ 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] = max(min(R[0, 0], max_reference), min_reference)
- U = numpy.clip(shooter.K * (R - shooter.X_hat), shooter.U_min, shooter.U_max)
+ 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.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]])
+ 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()
+ #pylab.plot(range(150), spin_down_x)
+ #pylab.show()
-
- if len(argv) != 3:
- print "Expected .cc file name and .h file name"
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
else:
- shooter.DumpHeaderFile(argv[1])
- shooter.DumpCppFile(argv[2], argv[1])
+ if argv[1][-3:] == '.cc':
+ print '.cc file is second'
+ else:
+ shooter.DumpHeaderFile(argv[1])
+ shooter.DumpCppFile(argv[2], argv[1])
if __name__ == '__main__':