Wrist and angle adjust now use delta U controllers.
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrist.py
index f26b99a..f21ed03 100755
--- a/frc971/control_loops/python/wrist.py
+++ b/frc971/control_loops/python/wrist.py
@@ -6,8 +6,8 @@
from matplotlib import pylab
class Wrist(control_loop.ControlLoop):
- def __init__(self):
- super(Wrist, self).__init__("Wrist")
+ def __init__(self, name="RawWrist"):
+ super(Wrist, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 1.4
# Stall Current in Amps
@@ -42,12 +42,10 @@
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
- self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
- self.dt, self.C)
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([.86, .46])
-
- print self.K
+ self.PlaceControllerPoles([0.85, 0.45])
self.rpl = .05
self.ipl = 0.008
@@ -57,9 +55,56 @@
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
+ self.InitializeState()
+
+
+class WristDeltaU(Wrist):
+ def __init__(self, name="Wrist"):
+ super(WristDeltaU, 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.37, 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.15])
+ 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(wrist, delta_u):
+ old_u = numpy.matrix([[wrist.X[2, 0]]])
+ new_u = numpy.clip(old_u + delta_u, wrist.U_min, wrist.U_max)
+ return new_u - old_u
+
def main(argv):
# Simulate the response of the system to a step input.
- wrist = Wrist()
+ wrist = WristDeltaU()
simulated_x = []
for _ in xrange(100):
wrist.Update(numpy.matrix([[12.0]]))
@@ -69,22 +114,36 @@
pylab.show()
# Simulate the closed loop response of the system to a step input.
- wrist = Wrist()
+ wrist = WristDeltaU()
close_loop_x = []
- R = numpy.matrix([[1.0], [0.0]])
+ close_loop_u = []
+ R = numpy.matrix([[1.0], [0.0], [0.0]])
+ wrist.X[2, 0] = -5
for _ in xrange(100):
U = numpy.clip(wrist.K * (R - wrist.X_hat), wrist.U_min, wrist.U_max)
+ U = ClipDeltaU(wrist, U)
wrist.UpdateObserver(U)
wrist.Update(U)
- close_loop_x.append(wrist.X[0, 0])
+ close_loop_x.append(wrist.X[0, 0] * 10)
+ close_loop_u.append(wrist.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"
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name for"
+ print "both the plant and unaugmented plant"
else:
+ unaug_wrist = Wrist("RawWrist")
+ unaug_loop_writer = control_loop.ControlLoopWriter("RawWrist",
+ [unaug_wrist])
+ 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("Wrist", [wrist])
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])