Worked on combined delta u controller for the claw. Doesn't work.
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index 3d6b9fc..b1f378e 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -1,6 +1,7 @@
#!/usr/bin/python
import control_loop
+import controls
import numpy
import sys
from matplotlib import pylab
@@ -32,28 +33,47 @@
# Control loop time step
self.dt = 0.01
+ # State is [bottom position, top - bottom position,
+ # bottom velocity, top - bottom velocity]
+ # Input is [bottom power, top power]
+ # Motor time constant.
+ self.motor_timeconstant = self.Kt / self.Kv / (self.J * self.G * self.G * self.R)
# State feedback matrices
self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ [[0, 0, 1, 0],
+ [0, 0, 0, 1],
+ [0, 0, -self.motor_timeconstant, 0],
+ [0, 0, 0, -self.motor_timeconstant]])
+
+ self.motor_feedforward = self.Kt / (self.J * 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]])
+ [[0, 0],
+ [0, 0],
+ [self.motor_feedforward, 0],
+ [-self.motor_feedforward, self.motor_feedforward]])
+ self.C = numpy.matrix([[1, 0, 0, 0],
+ [1, 1, 0, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.PlaceControllerPoles([0.85, 0.45])
+ #controlability = controls.ctrb(self.A, self.B);
+ #print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
+
+ self.PlaceControllerPoles([0.85, 0.45, 0.45, 0.85])
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl,
+ 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.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
self.InitializeState()
@@ -63,72 +83,138 @@
super(ClawDeltaU, self).__init__(name)
A_unaugmented = self.A
B_unaugmented = self.B
+ C_unaugmented = self.C
- 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.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 1.0]])
+ self.A[0:4, 0:4] = A_unaugmented
+ self.A[0:4, 4] = B_unaugmented[0:4, 0]
- self.B = numpy.matrix([[0.0],
- [0.0],
- [1.0]])
+ self.B = numpy.matrix([[0.0, 0.0],
+ [0.0, 0.0],
+ [0.0, 0.0],
+ [0.0, 0.0],
+ [1.0, 0.0]])
+ self.B[0:4, 1] = B_unaugmented[0:4, 1]
- self.C = numpy.matrix([[1.0, 0.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
+ self.C = numpy.concatenate((C_unaugmented, numpy.matrix([[0.0], [0.0]])),
+ axis=1)
+ self.D = numpy.matrix([[0.0, 0.0],
+ [0.0, 0.0]])
- self.PlaceControllerPoles([0.55, 0.35, 0.80])
+ #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
+ self.Q = numpy.matrix([[(1.0 / (0.04 ** 2.0)), 0.0, 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.01 ** 2)), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.01, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.08, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0 ** 2))]])
+
+ self.R = numpy.matrix([[0.000001, 0.0],
+ [0.0, 1.0 / (10.0 ** 2.0)]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
+ [50.0, 0.0, 10.0, 0.0, 1.0]])
+ #self.K = numpy.matrix([[50.0, -100.0, 0, -10, 0],
+ # [50.0, 100.0, 0, 10, 0]])
+
+ controlability = controls.ctrb(self.A, self.B);
+ print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(controlability)
print "K"
print self.K
print "Placed controller poles are"
print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ print [numpy.abs(x) for x in 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.PlaceObserverPoles([self.rpl + 1j * self.ipl, 0.10, 0.09,
self.rpl - 1j * self.ipl, 0.90])
- print "Placed observer poles are"
- print numpy.linalg.eig(self.A - self.L * self.C)[0]
+ #print "A is"
+ #print self.A
+ #print "L is"
+ #print self.L
+ #print "C is"
+ #print self.C
+ #print "A - LC is"
+ #print self.A - self.L * self.C
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ #print "Placed observer poles are"
+ #print numpy.linalg.eig(self.A - self.L * self.C)[0]
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
self.InitializeState()
-def ClipDeltaU(claw, delta_u):
- old_u = numpy.matrix([[claw.X[2, 0]]])
- new_u = numpy.clip(old_u + delta_u, claw.U_min, claw.U_max)
- return new_u - old_u
+def ClipDeltaU(claw, U):
+ delta_u = U[0, 0]
+ top_u = U[1, 0]
+ old_bottom_u = claw.X[4, 0]
+
+ # TODO(austin): Preserve the difference between the top and bottom power.
+ new_unclipped_bottom_u = old_bottom_u + delta_u
+
+ #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
+ if new_unclipped_bottom_u > claw.U_max[0, 0]:
+ #print "Bottom is too big. Was", new_unclipped_bottom_u, "changing top by", new_unclipped_bottom_u - claw.U_max[0, 0]
+ top_u -= new_unclipped_bottom_u - claw.U_max[0, 0]
+ new_unclipped_bottom_u = claw.U_max[0, 0]
+ if top_u > claw.U_max[1, 0]:
+ new_unclipped_bottom_u -= top_u - claw.U_max[1, 0]
+ top_u = claw.U_max[1, 0]
+ if top_u < claw.U_min[1, 0]:
+ new_unclipped_bottom_u -= top_u - claw.U_min[1, 0]
+ top_u = claw.U_min[1, 0]
+ if new_unclipped_bottom_u < claw.U_min[0, 0]:
+ top_u -= new_unclipped_bottom_u - claw.U_min[0, 0]
+ new_unclipped_bottom_u = claw.U_min[0, 0]
+
+ new_bottom_u = numpy.clip(new_unclipped_bottom_u, claw.U_min[0, 0], claw.U_max[0, 0])
+ new_top_u = numpy.clip(top_u, claw.U_min[1, 0], claw.U_max[1, 0])
+
+ return numpy.matrix([[new_bottom_u - old_bottom_u], [new_top_u]])
def main(argv):
# Simulate the response of the system to a step input.
- claw = ClawDeltaU()
- simulated_x = []
- for _ in xrange(100):
- claw.Update(numpy.matrix([[12.0]]))
- simulated_x.append(claw.X[0, 0])
+ #claw = ClawDeltaU()
+ #simulated_x = []
+ #for _ in xrange(100):
+ # claw.Update(numpy.matrix([[12.0]]))
+ # simulated_x.append(claw.X[0, 0])
- pylab.plot(range(100), simulated_x)
- pylab.show()
+ #pylab.plot(range(100), simulated_x)
+ #pylab.show()
# Simulate the closed loop response of the system to a step input.
top_claw = ClawDeltaU("TopClaw")
- close_loop_x = []
- close_loop_u = []
- R = numpy.matrix([[1.0], [0.0], [0.0]])
- top_claw.X[2, 0] = -5
- for _ in xrange(100):
- U = numpy.clip(top_claw.K * (R - top_claw.X_hat), top_claw.U_min, top_claw.U_max)
+ close_loop_x_bottom = []
+ close_loop_x_sep = []
+ close_loop_u_bottom = []
+ close_loop_u_top = []
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0], [0.0]])
+ top_claw.X[0, 0] = 0
+ for _ in xrange(50):
+ #print "Error is", (R - top_claw.X_hat)
+ U = top_claw.K * (R - top_claw.X_hat)
U = ClipDeltaU(top_claw, U)
top_claw.UpdateObserver(U)
top_claw.Update(U)
- close_loop_x.append(top_claw.X[0, 0] * 10)
- close_loop_u.append(top_claw.X[2, 0])
+ close_loop_x_bottom.append(top_claw.X[0, 0] * 10)
+ close_loop_u_bottom.append(top_claw.X[4, 0])
+ close_loop_x_sep.append(top_claw.X[1, 0] * 10)
+ close_loop_u_top.append(U[1, 0])
- pylab.plot(range(100), close_loop_x)
- pylab.plot(range(100), close_loop_u)
+ pylab.plot(range(50), close_loop_x_bottom, label='x bottom')
+ pylab.plot(range(50), close_loop_u_bottom, label='u bottom')
+ pylab.plot(range(50), close_loop_x_sep, label='seperation')
+ pylab.plot(range(50), close_loop_u_top, label='u top')
+ pylab.legend()
pylab.show()
# Write the generated constants out to a file.