Added python code to design the wrist control loop.
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 7d34a85..a40bfe2 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -81,3 +81,21 @@
num_uncontrollable_eigenvalues)
return K
+
+
+def c2d(A, B, dt):
+ """Converts from continuous time state space representation to discrete time.
+ Evaluates e^(A dt) for the discrete time version of A, and
+ integral(e^(A t) * B, 0, dt).
+ Returns (A, B). C and D are unchanged."""
+ e, P = numpy.linalg.eig(A)
+ diag = numpy.matrix(numpy.eye(A.shape[0]))
+ diage = numpy.matrix(numpy.eye(A.shape[0]))
+ for eig, count in zip(e, range(0, A.shape[0])):
+ diag[count, count] = numpy.exp(eig * dt)
+ if abs(eig) < 1.0e-16:
+ diage[count, count] = dt
+ else:
+ diage[count, count] = (numpy.exp(eig * dt) - 1.0) / eig
+
+ return (P * diag * numpy.linalg.inv(P), P * diage * numpy.linalg.inv(P) * B)
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrist.py
new file mode 100755
index 0000000..a2e46e4
--- /dev/null
+++ b/frc971/control_loops/python/wrist.py
@@ -0,0 +1,174 @@
+#!/usr/bin/python
+
+import numpy
+import string
+import sys
+import polytope
+from matplotlib import pylab
+import controls
+
+
+class Wrist(object):
+ def __init__(self):
+ # Stall Torque in N m
+ self.stall_torque = 1.4
+ # Stall Current in Amps
+ self.stall_current = 86
+ # Free Speed in RPM
+ self.free_speed = 6200.0
+ # Moment of inertia of the wrist in kg m^2
+ self.J = 0.51
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = (self.free_speed / 60.0 * 2.0 * numpy.pi) / (13.5 - self.R * 1.5)
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((84.0 / 20.0) * (50.0 / 14.0) * (40.0 / 14.0) * (40.0 / 12.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * 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]])
+
+ self.A, self.B = controls.c2d(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.K = controls.dplace(self.A, self.B, [.95, .92])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.L = controls.dplace(self.A.T, self.C.T,
+ [self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl]).T
+
+ self.X = numpy.matrix([[0],
+ [0]])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+ self.Y = self.C * self.X
+
+ def Update(self, U):
+ 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
+
+ def _DumpMatrix(self, matrix_name, matrix):
+ ans = [" Eigen::Matrix<double, %d, %d> %s;\n" % (
+ matrix.shape[0], matrix.shape[1], matrix_name)]
+ first = True
+ for element in numpy.nditer(matrix, order='C'):
+ if first:
+ ans.append(" %s << " % matrix_name)
+ first = False
+ else:
+ ans.append(", ")
+ ans.append(str(element))
+
+ ans.append(";\n")
+ return "".join(ans)
+
+ def DumpPlantHeader(self, plant_name):
+ """Writes out a c++ header declaration which will create a Plant object.
+
+ Args:
+ plant_name: string, the name of the plant. Used to create the name of the
+ function. The function name will be Make<plant_name>Plant().
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return "StateFeedbackPlant<%d, %d, %d> Make%sPlant();\n" % (
+ num_states, num_inputs, num_outputs, plant_name)
+
+ def DumpPlant(self, plant_name):
+ """Writes out a c++ function which will create a Plant object.
+
+ Args:
+ plant_name: string, the name of the plant. Used to create the name of the
+ function. The function name will be Make<plant_name>Plant().
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = ["StateFeedbackPlant<%d, %d, %d> Make%sPlant() {\n" % (
+ num_states, num_inputs, num_outputs, plant_name)]
+
+ ans.append(self._DumpMatrix("A", self.A))
+ ans.append(self._DumpMatrix("B", self.B))
+ ans.append(self._DumpMatrix("C", self.C))
+ ans.append(self._DumpMatrix("D", self.D))
+ ans.append(self._DumpMatrix("U_max", self.U_max))
+ ans.append(self._DumpMatrix("U_min", self.U_min))
+
+ ans.append(" return StateFeedbackPlant<%d, %d, %d>"
+ "(A, B, C, D, U_max, U_min);\n" % (num_states, num_inputs,
+ num_outputs))
+ ans.append("}\n")
+ return "".join(ans)
+
+
+def main(argv):
+ wrist = Wrist()
+ simulated_x = []
+ for _ in xrange(100):
+ wrist.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(wrist.X[0, 0])
+
+ pylab.plot(range(100), simulated_x)
+ pylab.show()
+
+ wrist = Wrist()
+ close_loop_x = []
+ X_hat = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(wrist.K * (R - X_hat), wrist.U_min, wrist.U_max)
+ X_hat = wrist.A * X_hat + wrist.B * U + wrist.L * (wrist.Y - wrist.C * X_hat - wrist.D * U)
+ wrist.Update(U)
+ close_loop_x.append(wrist.X[0, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.show()
+
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ namespace_start = ("namespace frc971 {\n"
+ "namespace control_loops {\n\n");
+
+ namespace_end = ("} // namespace frc971\n"
+ "} // namespace control_loops\n");
+
+ header_start = ("#ifndef FRC971_CONTROL_LOOPS_WRIST_MOTOR_PLANT_H_\n"
+ "#define // FRC971_CONTROL_LOOPS_WRIST_MOTOR_PLANT_H_\n\n")
+ header_end = "#endif // FRC971_CONTROL_LOOPS_WRIST_MOTOR_PLANT_H_\n";
+
+ with open(argv[1], "w") as fd:
+ fd.write(namespace_start)
+ fd.write(wrist.DumpPlant("Wrist"))
+ fd.write('\n')
+ fd.write(namespace_end)
+
+ with open(argv[2], "w") as fd:
+ fd.write(header_start)
+ fd.write(namespace_start)
+ fd.write("#include \"frc971/control_loops/state_feedback_loop.h\"\n")
+ fd.write('\n')
+ fd.write(wrist.DumpPlantHeader("Wrist"))
+ fd.write('\n')
+ fd.write(namespace_end)
+ fd.write(header_end)
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))