I updated the claw as best as I could to have independent separation/position. It won't work this way.
Currently, I had to make some simplifications in the model in order to be able to manipulate all 4 eigenvalues of the A - B * K matrix in the claw and those simplifications make the model inaccurate and therefore it doesn't actually properly control things and it just doesn't work. I am committing this for the sake of a record, but this should not atually be used.
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index bf70ad0..a801649 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -84,7 +84,7 @@
[self.motor_feedforward_difference]])
self.C = numpy.matrix([[1, 0, 0, 0],
- [1, 1, 0, 0]])
+ [1, 0, 1, 0]])
self.D = numpy.matrix([[0, 0],
[0, 0]])
@@ -96,7 +96,27 @@
self.A_diff, self.B_diff = controls.c2d(
self.A_diff_cont, self.B_diff_cont, self.dt)
- print self.A, self.B
+ print "A, A_bot, A_diff:"
+ print self.A, self.A_bottom, self.A_diff
+ print "B, B_bot, B_diff:"
+ print self.B, self.B_bottom, self.B_diff
+
+ # If B should equal [[B_bot, 0],[0, B_diff]], then the B
+ # generated by ContinuousToDiscrete adds in a couple extra
+ # numbers which make it impossible to control 4 values in K.
+ # Here, I make B equal what I had thought it should, thereby
+ # allowing us to control 4 values in K.
+ self.B_actual = numpy.matrix(self.B)
+ self.B[2, 0] = 0.0
+ self.B[3, 0] = 0.0
+ # If we do the above, with setting values of B to zero,
+ # then we can no longer make all the necessary values of A - B * K
+ # zero, because the discrete transform added a term affecting position
+ # over the timestep and that term can no longer be cancelled out if we
+ # are to also cancel out the term where the velocity of the bottom
+ # affects the velocity of the separation.
+ self.A_actual = numpy.matrix(self.A)
+ self.A[2, 1] = self.A[3, 1] * self.B[2, 1] / self.B[3, 1]
#controlability = controls.ctrb(self.A, self.B);
#print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
@@ -113,8 +133,8 @@
# TODO(james): Fix this for discrete time domain.
self.K = numpy.matrix([[0, 0, 0.0, 0.0],
[0.0, self.A[3, 1] / self.B[3, 1], 0, 0]])
- self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [0.5, 0.5])
- self.K_diff = controls.dplace(self.A_diff, self.B_diff, [0.5, 0.5])
+ self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [0.6, 0.7])
+ self.K_diff = controls.dplace(self.A_diff, self.B_diff, [0.3, 0.5])
self.K[0, 0:2] = self.K_bottom
self.K[1, 2:4] = self.K_diff
#lstsq_A = numpy.identity(2)
@@ -130,8 +150,6 @@
print "B * K unaugmented"
print self.B * self.K
F = self.A - self.B * self.K
- F[1, 2] = 0.0
- F[3, 2] = 0.0
print "A - B * K unaugmented"
print F
print "eigenvalues"
@@ -262,7 +280,7 @@
def AverageUFix(claw, U):
bottom_u = U[0, 0]
top_u = bottom_u + U[1, 0]
-#top_u = claw.J_top * (bottom_u / claw.J_bottom - U[1, 0])
+ top_u = bottom_u * claw.J_top / claw.J_bottom + U[1, 0]
#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[0, 0] or
@@ -271,8 +289,8 @@
top_u *= scalar
bottom_u *= scalar
- return numpy.matrix([[bottom_u], [top_u - bottom_u]])
- #return numpy.matrix([[bottom_u], [bottom_u / claw.J_bottom - top_u / claw.J_top]])
+ #return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+ return numpy.matrix([[bottom_u], [top_u - bottom_u * claw.J_top / claw.J_bottom]])
def ClipDeltaU(claw, U):
delta_u = U[0, 0]
@@ -318,31 +336,42 @@
t = []
close_loop_x_bottom = []
close_loop_x_sep = []
+ actual_sep = []
+ actual_x_bottom = []
close_loop_x_top = []
close_loop_u_bottom = []
close_loop_u_top = []
- R = numpy.matrix([[1.1], [0.05], [0.0], [0.0]])
- claw.X[0, 0] = 0
- claw.X[1, 0] = .05
+ R = numpy.matrix([[0.0], [0.00], [0.0], [0.0]])
+ claw.X[0, 0] = 1
+ claw.X_hat[0, 0] = 1
+ X_actual = claw.X
+ print "B actual"
+ print claw.B_actual
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 = 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)
+ #claw.Update(U)
+ X_actual = claw.A_actual * X_actual + claw.B_actual * U
+ claw.Y = claw.C * X_actual
+ close_loop_x_bottom.append(claw.X_hat[0, 0] * 10)
close_loop_u_bottom.append(U[0, 0])
- close_loop_x_sep.append(claw.X[1, 0] * 100)
- close_loop_x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10)
- close_loop_u_top.append(U[1, 0] + U[0, 0])
+ actual_sep.append(X_actual[2, 0] * 100)
+ actual_x_bottom.append(X_actual[0, 0] * 10)
+ close_loop_x_sep.append(claw.X_hat[2, 0] * 100)
+ close_loop_x_top.append((claw.X_hat[2, 0] + claw.X_hat[0, 0]) * 10)
+ close_loop_u_top.append(U[1, 0] + U[0, 0] * claw.J_top / claw.J_bottom)
t.append(0.01 * i)
pylab.plot(t, close_loop_x_bottom, label='x bottom')
pylab.plot(t, close_loop_x_sep, label='separation')
+ pylab.plot(t, actual_x_bottom, label='true x bottom')
+ pylab.plot(t, actual_sep, label='true separation')
pylab.plot(t, close_loop_x_top, label='x top')
pylab.plot(t, close_loop_u_bottom, label='u bottom')
pylab.plot(t, close_loop_u_top, label='u top')