Claw now zeros!
- Debugged plant
- Switched to a simple controller architecture.
- Fixed controller.
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index b1f378e..98d79ea 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -51,7 +51,7 @@
[[0, 0],
[0, 0],
[self.motor_feedforward, 0],
- [-self.motor_feedforward, self.motor_feedforward]])
+ [0.0, self.motor_feedforward]])
self.C = numpy.matrix([[1, 0, 0, 0],
[1, 1, 0, 0]])
self.D = numpy.matrix([[0, 0],
@@ -63,7 +63,17 @@
#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.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.03 ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, 0.2, 0.0],
+ [0.0, 0.0, 0.0, 2.00]])
+
+ self.R = numpy.matrix([[(1.0 / (20.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (20.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ print "K unaugmented"
+ print self.K
self.rpl = .05
self.ipl = 0.008
@@ -72,8 +82,8 @@
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl])
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.U_max = numpy.matrix([[12.0], [24.0]])
+ self.U_min = numpy.matrix([[-12.0], [-24.0]])
self.InitializeState()
@@ -152,6 +162,52 @@
self.InitializeState()
+def FullSeparationPriority(claw, U):
+ bottom_u = U[0, 0]
+ top_u = U[1, 0] + bottom_u
+
+ #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
+ if 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 -= bottom_u - claw.U_max[0, 0]
+ if top_u < claw.U_min[1, 0]:
+ top_u = claw.U_min[1, 0]
+
+ bottom_u = claw.U_max[0, 0]
+ if top_u > claw.U_max[1, 0]:
+ bottom_u -= top_u - claw.U_max[1, 0]
+ if bottom_u < claw.U_min[0, 0]:
+ bottom_u = claw.U_min[0, 0]
+
+ top_u = claw.U_max[1, 0]
+ if top_u < claw.U_min[1, 0]:
+ bottom_u -= top_u - claw.U_min[1, 0]
+ if bottom_u > claw.U_max[0, 0]:
+ bottom_u = claw.U_max[0, 0]
+
+ top_u = claw.U_min[1, 0]
+ if bottom_u < claw.U_min[0, 0]:
+ top_u -= bottom_u - claw.U_min[0, 0]
+ if top_u > claw.U_max[1, 0]:
+ top_u = claw.U_max[1, 0]
+
+ bottom_u = claw.U_min[0, 0]
+
+ return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+
+def AverageUFix(claw, U):
+ bottom_u = U[0, 0]
+ top_u = U[1, 0] + bottom_u
+
+ #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
+ if (bottom_u > claw.U_max[0, 0] or top_u > claw.U_max[1, 0] or
+ top_u < claw.U_min[1, 0] or bottom_u < claw.U_min[0, 0]):
+ scalar = 12.0 / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ top_u *= scalar
+ bottom_u *= scalar
+
+ return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+
def ClipDeltaU(claw, U):
delta_u = U[0, 0]
top_u = U[1, 0]
@@ -192,65 +248,47 @@
#pylab.show()
# Simulate the closed loop response of the system to a step input.
- top_claw = ClawDeltaU("TopClaw")
+ claw = Claw("TopClaw")
+ t = []
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_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])
+ R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+ claw.X[0, 0] = 0
+ for i in xrange(100):
+ #print "Error is", (R - claw.X_hat)
+ U = claw.K * (R - claw.X_hat)
+ #U = numpy.clip(claw.K * (R - claw.X_hat), claw.U_min, claw.U_max)
+ #U = FullSeparationPriority(claw, U)
+ U = AverageUFix(claw, U)
+ #U = claw.K * (R - claw.X_hat)
+ #U = ClipDeltaU(claw, U)
+ claw.UpdateObserver(U)
+ claw.Update(U)
+ close_loop_x_bottom.append(claw.X[0, 0] * 10)
+ close_loop_u_bottom.append(U[0, 0])
+ close_loop_x_sep.append(claw.X[1, 0] * 10)
+ close_loop_u_top.append(U[1, 0] + U[0, 0])
+ t.append(0.01 * i)
- 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.plot(t, close_loop_x_bottom, label='x bottom')
+ pylab.plot(t, close_loop_x_sep, label='seperation')
+ pylab.plot(t, close_loop_u_bottom, label='u bottom')
+ pylab.plot(t, close_loop_u_top, label='u top')
pylab.legend()
pylab.show()
# Write the generated constants out to a file.
- if len(argv) != 9:
- print "Expected .h file name and .cc file name for"
- print "both the plant and unaugmented plant"
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name for the claw."
else:
- top_unaug_claw = Claw("RawTopClaw")
- top_unaug_loop_writer = control_loop.ControlLoopWriter("RawTopClaw",
- [top_unaug_claw])
+ claw = Claw("Claw")
+ loop_writer = control_loop.ControlLoopWriter("Claw", [claw])
if argv[1][-3:] == '.cc':
- top_unaug_loop_writer.Write(argv[2], argv[1])
+ loop_writer.Write(argv[2], argv[1])
else:
- top_unaug_loop_writer.Write(argv[1], argv[2])
-
- top_loop_writer = control_loop.ControlLoopWriter("TopClaw", [top_claw])
- if argv[3][-3:] == '.cc':
- top_loop_writer.Write(argv[4], argv[3])
- else:
- top_loop_writer.Write(argv[3], argv[4])
-
- bottom_claw = ClawDeltaU("BottomClaw")
- bottom_unaug_claw = Claw("RawBottomClaw")
- bottom_unaug_loop_writer = control_loop.ControlLoopWriter(
- "RawBottomClaw", [bottom_unaug_claw])
- if argv[5][-3:] == '.cc':
- bottom_unaug_loop_writer.Write(argv[6], argv[5])
- else:
- bottom_unaug_loop_writer.Write(argv[5], argv[6])
-
- bottom_loop_writer = control_loop.ControlLoopWriter("BottomClaw",
- [bottom_claw])
- if argv[7][-3:] == '.cc':
- bottom_loop_writer.Write(argv[8], argv[7])
- else:
- bottom_loop_writer.Write(argv[7], argv[8])
+ loop_writer.Write(argv[1], argv[2])
if __name__ == '__main__':
sys.exit(main(sys.argv))