Added James' wrist control loop code.
diff --git a/frc971/control_loops/python/wrist.py b/frc971/control_loops/python/wrist.py
index e239ac7..441999f 100755
--- a/frc971/control_loops/python/wrist.py
+++ b/frc971/control_loops/python/wrist.py
@@ -1,27 +1,28 @@
#!/usr/bin/python
+import control_loop
import numpy
-import string
import sys
-import polytope
from matplotlib import pylab
-import controls
-
-class Wrist(object):
+class Wrist(control_loop.ControlLoop):
def __init__(self):
+ super(Wrist, self).__init__("Wrist")
# 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
+ # Free Current in Amps
+ self.free_current = 1.5
# 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)
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
@@ -39,181 +40,49 @@
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.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
- self.K = controls.dplace(self.A, self.B, [.89, .85])
+ self.PlaceControllerPoles([.89, .85])
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.PlaceObserverPoles([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.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 DumpLoopHeader(self, loop_name):
- """Writes out a c++ header declaration which will create a Loop object.
-
- Args:
- loop_name: string, the name of the loop. Used to create the name of the
- function. The function name will be Make<loop_name>Loop().
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return "StateFeedbackLoop<%d, %d, %d> Make%sLoop();\n" % (
- num_states, num_inputs, num_outputs, loop_name)
-
- def DumpLoop(self, loop_name):
- """Writes out a c++ function which will create a Loop object.
-
- Args:
- loop_name: string, the name of the loop. Used to create the name of the
- function and create the plant. The function name will be
- Make<loop_name>Loop().
- """
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ["StateFeedbackLoop<%d, %d, %d> Make%sLoop() {\n" % (
- num_states, num_inputs, num_outputs, loop_name)]
-
- ans.append(self._DumpMatrix("L", self.L))
- ans.append(self._DumpMatrix("K", self.K))
-
- ans.append(" return StateFeedbackLoop<%d, %d, %d>"
- "(L, K, Make%sPlant());\n" % (num_states, num_inputs,
- num_outputs, loop_name))
- ans.append("}\n")
- return "".join(ans)
-
def main(argv):
+ # Simulate the response of the system to a step input.
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()
+ pylab.plot(range(100), simulated_x)
+ pylab.show()
+ # Simulate the closed loop response of the system to a step input.
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)
+ U = numpy.clip(wrist.K * (R - wrist.X_hat), wrist.U_min, wrist.U_max)
+ wrist.UpdateObserver(U)
wrist.Update(U)
close_loop_x.append(wrist.X[0, 0])
pylab.plot(range(100), close_loop_x)
pylab.show()
+ # Write the generated constants out to a file.
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("#include \"frc971/control_loops/wrist_motor_plant.h\"\n")
- fd.write('\n')
- fd.write("#include \"frc971/control_loops/state_feedback_loop.h\"\n")
- fd.write('\n')
- fd.write(namespace_start)
- fd.write('\n')
- fd.write(wrist.DumpPlant("Wrist"))
- fd.write('\n')
- fd.write(wrist.DumpLoop("Wrist"))
- fd.write('\n')
- fd.write(namespace_end)
-
- with open(argv[2], "w") as fd:
- fd.write(header_start)
- fd.write("#include \"frc971/control_loops/state_feedback_loop.h\"\n")
- fd.write('\n')
- fd.write(namespace_start)
- fd.write(wrist.DumpPlantHeader("Wrist"))
- fd.write('\n')
- fd.write(wrist.DumpLoopHeader("Wrist"))
- fd.write('\n')
- fd.write(namespace_end)
- fd.write(header_end)
-
+ wrist.DumpHeaderFile(argv[1])
+ wrist.DumpCppFile(argv[2], argv[1])
if __name__ == '__main__':
sys.exit(main(sys.argv))