It seems to work reasonably. Still need to make it perfect.
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index a801649..b0767fb 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -40,14 +40,14 @@
# bottom velocity affects the difference of the top and bottom velocities.
self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
self.bottom_bottom = self.common_motor_constant / self.J_bottom
- self.difference_bottom = self.common_motor_constant * (1 / self.J_bottom
+ self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom
- 1 / self.J_top)
self.difference_difference = self.common_motor_constant / self.J_top
# State feedback matrices
self.A_continuous = numpy.matrix(
- [[0, 0, 0, 0],
- [0, 0, 0, 0],
+ [[0, 0, 1, 0],
+ [0, 0, 0, 1],
[0, 0, self.bottom_bottom, 0],
[0, 0, self.difference_bottom, self.difference_difference]])
@@ -59,9 +59,9 @@
[[0, 1],
[0, self.difference_difference]])
- self.A_continuous[0:2, 0:2] = self.A_bottom_cont
- self.A_continuous[2:4, 2:4] = self.A_diff_cont
- self.A_continuous[3, 1] = self.difference_bottom
+ # self.A_continuous[0:2, 0:2] = self.A_bottom_cont
+ # self.A_continuous[2:4, 2:4] = self.A_diff_cont
+ # self.A_continuous[3, 1] = self.difference_bottom
self.motor_feedforward = self.Kt / (self.G * self.R)
self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
@@ -70,11 +70,13 @@
self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
self.B_continuous = numpy.matrix(
[[0, 0],
- [self.motor_feedforward_bottom, 0],
[0, 0],
- [0,#self.motor_feedforward_difference_bottom,
+ [self.motor_feedforward_bottom, 0],
+ [-self.motor_feedforward_bottom,
self.motor_feedforward_difference]])
+ print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant
+
self.B_bottom_cont = numpy.matrix(
[[0],
[self.motor_feedforward_bottom]])
@@ -84,7 +86,7 @@
[self.motor_feedforward_difference]])
self.C = numpy.matrix([[1, 0, 0, 0],
- [1, 0, 1, 0]])
+ [1, 1, 0, 0]])
self.D = numpy.matrix([[0, 0],
[0, 0]])
@@ -96,28 +98,27 @@
self.A_diff, self.B_diff = controls.c2d(
self.A_diff_cont, self.B_diff_cont, self.dt)
- 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]
+ print "A"
+ print self.A
+ print "B"
+ print self.B
+ X_ss = numpy.matrix([[0], [0], [0.0], [0]])
+
+ U = numpy.matrix([[1.0],[1.0]])
+ A = self.A
+ B = self.B
+ #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
+ X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
+ #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+ #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
+ X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
+
+ print "X_ss", X_ss
+
#controlability = controls.ctrb(self.A, self.B);
#print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
@@ -130,20 +131,20 @@
[0.0, (1.0 / (5.0 ** 2.0))]])
#self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- # 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.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)
- #lstsq_A[0] = self.B[1]
- #lstsq_A[1] = self.B[3]
+ self.K = numpy.matrix([[50, 0.0, 1, 0.0],
+ [0.0, 300, 0.0, 1.1]])
+ lstsq_A = numpy.identity(2)
+ lstsq_A[0] = self.B[1]
+ lstsq_A[1] = self.B[3]
+ print "System of Equations coefficients:"
+ print lstsq_A
+ print "det", numpy.linalg.det(lstsq_A)
+ self.K[1, 0] = -lstsq_A[0, 0] * self.K[0, 0] / lstsq_A[0, 1]
#self.K[0:2, 0] = numpy.linalg.lstsq(lstsq_A, numpy.matrix([[0.0], [0.0]]))[0]
- #self.K[0:2, 2] = numpy.linalg.lstsq(
- # lstsq_A,
- # numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
+ out_x = numpy.linalg.lstsq(
+ lstsq_A,
+ numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
+ self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
print "K unaugmented"
print self.K
@@ -277,20 +278,47 @@
return numpy.matrix([[bottom_u], [top_u - bottom_u]])
-def AverageUFix(claw, U):
- bottom_u = U[0, 0]
- top_u = bottom_u + U[1, 0]
- top_u = bottom_u * claw.J_top / claw.J_bottom + U[1, 0]
+def AverageUFix(claw, U, preserve_v3=False):
+ """Clips U as necessary.
- #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
- top_u < claw.U_min[0, 0] or bottom_u < claw.U_min[0, 0]):
- scalar = 12.0 / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ Args:
+ claw: claw object containing moments of inertia and U limits.
+ U: Input matrix to clip as necessary.
+ preserve_v3: There are two ways to attempt to clip the voltages:
+ -If you preserve the imaginary v3, then it will attempt to
+ preserve the effect on the separation of the two claws.
+ If it is not able to do this, then it calls itself with preserve_v3
+ set to False.
+ -If you preserve the ratio of the voltage of the bottom and the top,
+ then it will attempt to preserve the ratio of those two. This is
+ equivalent to preserving the ratio of v3 and the bottom voltage.
+ """
+ bottom_u = U[0, 0]
+ top_u = U[1, 0]
+ seperation_u = top_u - bottom_u * claw.J_top / claw.J_bottom
+
+ top_big = top_u > claw.U_max[0, 0]
+ top_small = top_u < claw.U_min[0, 0]
+ bot_big = bottom_u > claw.U_max[0, 0]
+ bot_small = top_u < claw.U_min[0, 0]
+ bottom_bad = bot_big or bot_small
+ top_bad = top_big or top_small
+ scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ if bottom_bad and preserve_v3:
+ bottom_u *= scalar
+ top_u = seperation_u + bottom_u * claw.J_top / claw.J_bottom
+ if abs(top_u) > claw.U_max[0, 0]:
+ return AverageUFix(claw, U, preserve_v3=False)
+ elif top_bad and preserve_v3:
+ top_u *= scalar
+ bottom_u = (top_u - seperation_u) * claw.J_bottom / claw.J_top
+ if abs(bottom_u) > claw.U_max[0, 0]:
+ return AverageUFix(claw, U, preserve_v3=False)
+ elif (bottom_bad or top_bad) and not preserve_v3:
top_u *= scalar
bottom_u *= scalar
- #return numpy.matrix([[bottom_u], [top_u - bottom_u]])
- return numpy.matrix([[bottom_u], [top_u - bottom_u * claw.J_top / claw.J_bottom]])
+ return numpy.matrix([[bottom_u], [top_u]])
def ClipDeltaU(claw, U):
delta_u = U[0, 0]
@@ -343,35 +371,34 @@
close_loop_u_top = []
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
+ claw.X[1, 0] = .0
+ claw.X_hat = claw.X
+ #X_actual = claw.X
for i in xrange(100):
#print "Error is", (R - claw.X_hat)
- U = claw.K * (R - claw.X_hat)
+ U = claw.K * (R - claw.X)
#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, preserve_v3=True)
#U = claw.K * (R - claw.X_hat)
#U = ClipDeltaU(claw, U)
claw.UpdateObserver(U)
- #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)
+ 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[0, 0] * 10)
close_loop_u_bottom.append(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)
+ #actual_sep.append(X_actual[2, 0] * 100)
+ #actual_x_bottom.append(X_actual[0, 0] * 10)
+ 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])
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, 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')
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 4b63aec..0610225 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -183,7 +183,7 @@
def Update(self, U):
"""Simulates one time step with the provided U."""
- U = numpy.clip(U, self.U_min, self.U_max)
+ #U = numpy.clip(U, self.U_min, self.U_max)
self.X = self.A * self.X + self.B * U
self.Y = self.C * self.X + self.D * U