Fixed shooter to not have a spring some times.
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 967be3c..64bd52d 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -85,7 +85,7 @@
def c2d(A, B, dt):
"""Converts from continuous time state space representation to discrete time.
- Evaluates e^(A dt) for the discrete time version of A, and
+ Evaluates e^(A dt) - I for the discrete time version of A, and
integral(e^(A t) * B, 0, dt).
Returns (A, B). C and D are unchanged."""
e, P = numpy.linalg.eig(A)
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 3c411fb..c2b9dbb 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -5,9 +5,9 @@
import sys
from matplotlib import pylab
-class Shooter(control_loop.ControlLoop):
- def __init__(self, name="RawShooter"):
- super(Shooter, self).__init__(name)
+class SprungShooter(control_loop.ControlLoop):
+ def __init__(self, name="RawSprungShooter"):
+ super(SprungShooter, self).__init__(name)
# Stall Torque in N m
self.stall_torque = .4982
# Stall Current in Amps
@@ -20,7 +20,7 @@
# This rough estimate should about include the effect of the masses
# of the gears. If this number is too low, the eigen values of self.A
# will start to become extremely small.
- self.J = 12
+ self.J = 20
# Resistance of the motor, divided by the number of motors.
self.R = 12.0 / self.stall_current / 2.0
# Motor velocity constant
@@ -31,11 +31,11 @@
# Spring constant for the springs, N/m
self.Ks = 2800.0
# Gear ratio multiplied by radius of final sprocket.
- self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 0.0182
+ self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
+
# Control loop time step
self.dt = 0.01
-
# State feedback matrices
self.A_continuous = numpy.matrix(
[[0, 1],
@@ -63,6 +63,73 @@
self.InitializeState()
+class Shooter(SprungShooter):
+ def __init__(self, name="RawShooter"):
+ super(Shooter, self).__init__(name)
+
+ # 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.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([0.45, 0.45])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl,
+ self.rpl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
+
+class SprungShooterDeltaU(SprungShooter):
+ def __init__(self, name="SprungShooter"):
+ super(SprungShooterDeltaU, 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.45, 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()
+
+
class ShooterDeltaU(Shooter):
def __init__(self, name="Shooter"):
super(ShooterDeltaU, self).__init__(name)
@@ -102,39 +169,50 @@
self.InitializeState()
-def ClipDeltaU(shooter, delta_u):
- old_u = numpy.matrix([[shooter.X[2, 0]]])
+def ClipDeltaU(shooter, old_voltage, delta_u):
+ old_u = old_voltage
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 = Shooter()
- simulated_x = []
- for _ in xrange(2000):
- U = 2.0
- shooter.Update(numpy.matrix([[U]]))
- simulated_x.append(shooter.X[0, 0])
-
- pylab.plot(range(2000), simulated_x)
- pylab.show()
-
# Simulate the response of the system to a goal.
- shooter = Shooter()
+ sprung_shooter = SprungShooterDeltaU()
+ raw_sprung_shooter = SprungShooter()
close_loop_x = []
close_loop_u = []
- R = numpy.matrix([[0.3], [0.0]])
+ goal_position = -0.3
+ R = numpy.matrix([[goal_position], [0.0], [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] * goal_position]])
+ voltage = numpy.matrix([[0.0]])
for _ in xrange(500):
- augment = (-numpy.linalg.lstsq(shooter.B_continuous, numpy.identity(
- shooter.B_continuous.shape[0]))[0] *
- shooter.A_continuous * R)
- U = numpy.clip(shooter.K * (R - shooter.X_hat) + augment,
- shooter.U_min, shooter.U_max)
-#U = ClipDeltaU(shooter, U)
+ U = sprung_shooter.K * (R - sprung_shooter.X_hat)
+ U = ClipDeltaU(sprung_shooter, voltage, U)
+ sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
+ sprung_shooter.UpdateObserver(U)
+ voltage += U;
+ raw_sprung_shooter.Update(voltage)
+ close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
+ close_loop_u.append(voltage[0, 0])
+
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
+ pylab.show()
+
+ shooter = ShooterDeltaU()
+ raw_shooter = Shooter()
+ close_loop_x = []
+ close_loop_u = []
+ goal_position = -0.3
+ R = numpy.matrix([[goal_position], [0.0], [-shooter.A[1, 0] / shooter.A[1, 2] * goal_position]])
+ voltage = numpy.matrix([[0.0]])
+ for _ in xrange(500):
+ U = shooter.K * (R - shooter.X_hat)
+ U = ClipDeltaU(shooter, voltage, U)
+ shooter.Y = raw_shooter.Y + 0.01
shooter.UpdateObserver(U)
- shooter.Update(U)
- close_loop_x.append(shooter.X[0, 0] * 10)
- close_loop_u.append(U[0, 0])
+ voltage += U;
+ raw_shooter.Update(voltage)
+ close_loop_x.append(raw_shooter.X[0, 0] * 10)
+ close_loop_u.append(voltage[0, 0])
pylab.plot(range(500), close_loop_x)
pylab.plot(range(500), close_loop_u)
@@ -145,16 +223,19 @@
print "Expected .h file name and .cc file name for"
print "both the plant and unaugmented plant"
else:
+ unaug_sprung_shooter = SprungShooter("RawSprungShooter")
unaug_shooter = Shooter("RawShooter")
unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
- [unaug_shooter])
+ [unaug_sprung_shooter,
+ unaug_shooter])
if argv[3][-3:] == '.cc':
unaug_loop_writer.Write(argv[4], argv[3])
else:
unaug_loop_writer.Write(argv[3], argv[4])
+ sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter, shooter])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else: