Added control loops for all subsystems and made tests run.
Change-Id: I66542db4355a89f6d24c1ad4772004182197c863
diff --git a/frc971/control_loops/python/arm.py b/frc971/control_loops/python/arm.py
new file mode 100755
index 0000000..c910f4c
--- /dev/null
+++ b/frc971/control_loops/python/arm.py
@@ -0,0 +1,249 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import polytope
+import polydrivetrain
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+
+class Arm(control_loop.ControlLoop):
+ def __init__(self, name="Arm", mass=None):
+ super(Arm, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.476
+ # Stall Current in Amps
+ self.stall_current = 80.730
+ # Free Speed in RPM
+ self.free_speed = 13906.0
+ # Free Current in Amps
+ self.free_current = 5.820
+ # Mass of the arm
+ if mass is None:
+ self.mass = 13.0
+ else:
+ self.mass = mass
+
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = (44.0 / 12.0) * (54.0 / 14.0) * (54.0 / 14.0) * (44.0 / 20.0) * (72.0 / 16.0)
+ # Fridge arm length
+ self.r = 32 * 0.0254
+ # Control loop time step
+ self.dt = 0.005
+
+ # Arm moment of inertia
+ self.J = self.r * self.mass
+
+ # Arm left/right spring constant (N*m / radian)
+ self.spring = 3000.0
+
+ # State is [average position, average velocity,
+ # position difference/2, velocity difference/2]
+ # Position difference is 1 - 2
+ # Input is [Voltage 1, Voltage 2]
+
+ C1 = self.spring / (self.J * 0.5)
+ C2 = self.Kt * self.G / (self.J * 0.5 * self.R)
+ C3 = self.G * self.G * self.Kt / (self.R * self.J * 0.5 * self.Kv)
+
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, -C3, 0, 0],
+ [0, 0, 0, 1],
+ [0, 0, -C1 * 2.0, -C3]])
+
+ print 'Full speed is', C2 / C3 * 12.0
+
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [C2 / 2.0, C2 / 2.0],
+ [0, 0],
+ [C2 / 2.0, -C2 / 2.0]])
+
+ self.C = numpy.matrix([[1, 0, 1, 0],
+ [1, 0, -1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ controlability = controls.ctrb(self.A, self.B);
+ print 'Rank of augmented controlability matrix.', numpy.linalg.matrix_rank(
+ controlability)
+
+ q_pos = 0.02
+ q_vel = 0.300
+ q_pos_diff = 0.01
+ q_vel_diff = 0.15
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, 1.0 / (12.0 ** 2.0)]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ print 'Controller'
+ print self.K
+
+ print 'Controller Poles'
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = 0.20
+ self.ipl = 0.05
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+def CapU(U):
+ if U[0, 0] - U[1, 0] > 24:
+ return numpy.matrix([[12], [-12]])
+ elif U[0, 0] - U[1, 0] < -24:
+ return numpy.matrix([[-12], [12]])
+ else:
+ max_u = max(U[0, 0], U[1, 0])
+ min_u = min(U[0, 0], U[1, 0])
+ if max_u > 12:
+ return U - (max_u - 12)
+ if min_u < -12:
+ return U - (min_u + 12)
+ return U
+
+
+def run_test(arm, initial_X, goal, max_separation_error=0.01,
+ show_graph=True, iterations=200, controller_arm=None,
+ observer_arm=None):
+ """Runs the arm plant with an initial condition and goal.
+
+ The tests themselves are not terribly sophisticated; I just test for
+ whether the goal has been reached and whether the separation goes
+ outside of the initial and goal values by more than max_separation_error.
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ arm: arm object to use.
+ initial_X: starting state.
+ goal: goal state.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for.
+ controller_arm: arm object to get K from, or None if we should
+ use arm.
+ observer_arm: arm object to use for the observer, or None if we should
+ use the actual state.
+ """
+
+ arm.X = initial_X
+
+ if controller_arm is None:
+ controller_arm = arm
+
+ if observer_arm is not None:
+ observer_arm.X_hat = initial_X + 0.01
+ observer_arm.X_hat = initial_X
+
+ # Various lists for graphing things.
+ t = []
+ x_avg = []
+ x_sep = []
+ x_hat_avg = []
+ x_hat_sep = []
+ v_avg = []
+ v_sep = []
+ u_left = []
+ u_right = []
+
+ sep_plot_gain = 100.0
+
+ for i in xrange(iterations):
+ X_hat = arm.X
+ if observer_arm is not None:
+ X_hat = observer_arm.X_hat
+ x_hat_avg.append(observer_arm.X_hat[0, 0])
+ x_hat_sep.append(observer_arm.X_hat[2, 0] * sep_plot_gain)
+ U = controller_arm.K * (goal - X_hat)
+ U = CapU(U)
+ x_avg.append(arm.X[0, 0])
+ v_avg.append(arm.X[1, 0])
+ x_sep.append(arm.X[2, 0] * sep_plot_gain)
+ v_sep.append(arm.X[3, 0])
+ if observer_arm is not None:
+ observer_arm.PredictObserver(U)
+ arm.Update(U)
+ if observer_arm is not None:
+ observer_arm.Y = arm.Y
+ observer_arm.CorrectObserver(U)
+
+ t.append(i * arm.dt)
+ u_left.append(U[0, 0])
+ u_right.append(U[1, 0])
+
+ print numpy.linalg.inv(arm.A)
+ print "delta time is ", arm.dt
+ print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
+ print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+
+ if show_graph:
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x_avg, label='x avg')
+ pylab.plot(t, x_sep, label='x sep')
+ if observer_arm is not None:
+ pylab.plot(t, x_hat_avg, label='x_hat avg')
+ pylab.plot(t, x_hat_sep, label='x_hat sep')
+ pylab.legend()
+
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u_left, label='u left')
+ pylab.plot(t, u_right, label='u right')
+ pylab.legend()
+ pylab.show()
+
+
+def main(argv):
+ loaded_mass = 25
+ #loaded_mass = 0
+ arm = Arm(mass=13 + loaded_mass)
+ arm_controller = Arm(mass=13 + 15)
+ observer_arm = Arm(mass=13 + 15)
+ #observer_arm = None
+
+ # Test moving the arm with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
+ #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(arm, initial_X, R, controller_arm=arm_controller,
+ observer_arm=observer_arm)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name for the arm."
+ else:
+ arm = Arm("Arm")
+ loop_writer = control_loop.ControlLoopWriter("Arm", [arm])
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index b5ea6a1..1a25f95 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -6,357 +6,99 @@
import polydrivetrain
import numpy
import sys
+import matplotlib
from matplotlib import pylab
class Claw(control_loop.ControlLoop):
- def __init__(self, name="RawClaw"):
+ def __init__(self, name="Claw", mass=None):
super(Claw, self).__init__(name)
# Stall Torque in N m
- self.stall_torque = 2.42
+ self.stall_torque = 0.476
# Stall Current in Amps
- self.stall_current = 133
+ self.stall_current = 80.730
# Free Speed in RPM
- self.free_speed = 5500.0
+ self.free_speed = 13906.0
# Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the claw in kg m^2
- self.J_top = 2.8
- self.J_bottom = 3.0
+ self.free_current = 5.820
+ # Mass of the claw
+ if mass is None:
+ self.mass = 5.0
+ else:
+ self.mass = mass
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (13.5 - self.R * self.free_current))
+ (12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
- self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
- # Control loop time step
- self.dt = 0.01
+ self.G = (56.0 / 12.0) * (54.0 / 14.0) * (64.0 / 14.0) * (72.0 / 18.0)
+ # Claw length
+ self.r = 18 * 0.0254
- # State is [bottom position, bottom velocity, top - bottom position,
- # top - bottom velocity]
- # Input is [bottom power, top power - bottom power * J_top / J_bottom]
- # Motor time constants. difference_bottom refers to the constant for how the
- # 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
- - 1 / self.J_top)
- self.difference_difference = self.common_motor_constant / self.J_top
- # State feedback matrices
+ self.J = self.r * self.mass
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # State is [position, velocity]
+ # Input is [Voltage]
+
+ C1 = self.G * self.G * self.Kt / (self.R * self.J * self.Kv)
+ C2 = self.Kt * self.G / (self.J * self.R)
self.A_continuous = numpy.matrix(
- [[0, 0, 1, 0],
- [0, 0, 0, 1],
- [0, 0, self.bottom_bottom, 0],
- [0, 0, self.difference_bottom, self.difference_difference]])
-
- self.A_bottom_cont = numpy.matrix(
[[0, 1],
- [0, self.bottom_bottom]])
+ [0, -C1]])
- self.A_diff_cont = numpy.matrix(
- [[0, 1],
- [0, self.difference_difference]])
-
- self.motor_feedforward = self.Kt / (self.G * self.R)
- self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
- self.motor_feedforward_difference = self.motor_feedforward / self.J_top
- self.motor_feedforward_difference_bottom = (
- self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
+ # Start with the unmodified input
self.B_continuous = numpy.matrix(
- [[0, 0],
- [0, 0],
- [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]])
+ [C2]])
- self.B_diff_cont = numpy.matrix(
- [[0],
- [self.motor_feedforward_difference]])
-
- self.C = numpy.matrix([[1, 0, 0, 0],
- [1, 1, 0, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.A_bottom, self.B_bottom = controls.c2d(
- self.A_bottom_cont, self.B_bottom_cont, self.dt)
- self.A_diff, self.B_diff = controls.c2d(
- self.A_diff_cont, self.B_diff_cont, self.dt)
-
- self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.75 + 0.1j, .75 - 0.1j])
- self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.75 + 0.1j, .75 - 0.1j])
-
- print "K_diff", self.K_diff
- print "K_bottom", self.K_bottom
-
- print "A"
print self.A
- print "B"
- print self.B
-
-
- self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, 0.10, 0.0],
- [0.0, 0.0, 0.0, 0.1]])
-
- self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (5.0 ** 2.0))]])
- #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
-
- self.K = numpy.matrix([[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
- [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
-
- # Compute the feed forwards aceleration term.
- self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[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)
-
- 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
- print "B * K unaugmented"
- print self.B * self.K
- F = self.A - self.B * self.K
- print "A - B * K unaugmented"
- print F
- print "eigenvalues"
- print numpy.linalg.eig(F)[0]
-
- self.rpl = .05
- self.ipl = 0.010
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
- self.rpl - 1j * self.ipl])
-
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
-
- # For the tests that check the limits, these are (upper, lower) for both
- # claws.
- self.hard_pos_limits = None
- self.pos_limits = None
-
- # Compute the steady state velocities for a given applied voltage.
- # The top and bottom of the claw should spin at the same rate if the
- # physics is right.
- 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
-
- self.InitializeState()
-
-
-class ClawDeltaU(Claw):
- def __init__(self, name="Claw"):
- super(ClawDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
- C_unaugmented = self.C
-
- self.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 1.0]])
- self.A[0:4, 0:4] = A_unaugmented
- self.A[0:4, 4] = B_unaugmented[0:4, 0]
-
- self.B = numpy.matrix([[0.0, 0.0],
- [0.0, 0.0],
- [0.0, 0.0],
- [0.0, 0.0],
- [1.0, 0.0]])
- self.B[0:4, 1] = B_unaugmented[0:4, 1]
-
- self.C = numpy.concatenate((C_unaugmented, numpy.matrix([[0.0], [0.0]])),
- axis=1)
- self.D = numpy.matrix([[0.0, 0.0],
- [0.0, 0.0]])
-
- #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
- self.Q = numpy.matrix([[(1.0 / (0.04 ** 2.0)), 0.0, 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.01 ** 2)), 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.01, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.08, 0.0],
- [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0 ** 2))]])
-
- self.R = numpy.matrix([[0.000001, 0.0],
- [0.0, 1.0 / (10.0 ** 2.0)]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
-
- self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
- [50.0, 0.0, 10.0, 0.0, 1.0]])
- #self.K = numpy.matrix([[50.0, -100.0, 0, -10, 0],
- # [50.0, 100.0, 0, 10, 0]])
controlability = controls.ctrb(self.A, self.B);
print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(controlability)
- print "K"
+ q_pos = 0.03
+ q_vel = 0.5
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
print self.K
- print "Placed controller poles are"
+
print numpy.linalg.eig(self.A - self.B * self.K)[0]
- print [numpy.abs(x) for x in numpy.linalg.eig(self.A - self.B * self.K)[0]]
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl, 0.10, 0.09,
- self.rpl - 1j * self.ipl, 0.90])
- #print "A is"
- #print self.A
- #print "L is"
- #print self.L
- #print "C is"
- #print self.C
- #print "A - LC is"
- #print self.A - self.L * self.C
+ self.rpl = 0.20
+ self.ipl = 0.05
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
- #print "Placed observer poles are"
- #print numpy.linalg.eig(self.A - self.L * self.C)[0]
-
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
self.InitializeState()
-def ScaleU(claw, U, K, error):
- """Clips U as necessary.
- Args:
- claw: claw object containing moments of inertia and U limits.
- U: Input matrix to clip as necessary.
- """
+def run_test(claw, initial_X, goal, max_separation_error=0.01,
+ show_graph=True, iterations=200, controller_claw=None,
+ observer_claw=None):
+ """Runs the claw plant with an initial condition and goal.
- bottom_u = U[0, 0]
- top_u = U[1, 0]
- position_error = error[0:2, 0]
- velocity_error = error[2:, 0]
-
- U_poly = polytope.HPolytope(
- numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]]),
- numpy.matrix([[12],
- [12],
- [12],
- [12]]))
-
- if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
- top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
-
- position_K = K[:, 0:2]
- velocity_K = K[:, 2:]
-
- # H * U <= k
- # U = UPos + UVel
- # H * (UPos + UVel) <= k
- # H * UPos <= k - H * UVel
- #
- # Now, we can do a coordinate transformation and say the following.
- #
- # UPos = position_K * position_error
- # (H * position_K) * position_error <= k - H * UVel
- #
- # Add in the constraint that 0 <= t <= 1
- # Now, there are 2 ways this can go. Either we have a region, or we don't
- # have a region. If we have a region, then pick the largest t and go for it.
- # If we don't have a region, we need to pick a good comprimise.
-
- pos_poly = polytope.HPolytope(
- U_poly.H * position_K,
- U_poly.k - U_poly.H * velocity_K * velocity_error)
-
- # The actual angle for the line we call 45.
- angle_45 = numpy.matrix([[numpy.sqrt(3), 1]])
- if claw.pos_limits and claw.hard_pos_limits and claw.X[0, 0] + claw.X[1, 0] > claw.pos_limits[1]:
- angle_45 = numpy.matrix([[1, 1]])
-
- P = position_error
- L45 = numpy.multiply(numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]]), angle_45)
- if L45[0, 1] == 0:
- L45[0, 1] = 1
- if L45[0, 0] == 0:
- L45[0, 0] = 1
- w45 = numpy.matrix([[0]])
-
- if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
- LH = numpy.matrix([[0, 1]])
- else:
- LH = numpy.matrix([[1, 0]])
- wh = LH * P
- standard = numpy.concatenate((L45, LH))
- W = numpy.concatenate((w45, wh))
- intersection = numpy.linalg.inv(standard) * W
- adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(pos_poly,
- LH, wh, position_error)
- adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(pos_poly,
- L45, w45, intersection)
- if pos_poly.IsInside(intersection):
- adjusted_pos_error = adjusted_pos_error_h
- else:
- if is_inside_h:
- if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(adjusted_pos_error_45):
- adjusted_pos_error = adjusted_pos_error_h
- else:
- adjusted_pos_error = adjusted_pos_error_45
- else:
- adjusted_pos_error = adjusted_pos_error_45
- #print adjusted_pos_error
-
- #print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
- return velocity_K * velocity_error + position_K * adjusted_pos_error
-
- #U = Kpos * poserror + Kvel * velerror
-
- #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
-
- #top_u *= scalar
- #bottom_u *= scalar
-
- return numpy.matrix([[bottom_u], [top_u]])
-
-def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=False, iterations=200):
- """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
-
- The tests themselves are not terribly sophisticated; I just test for
+ The tests themselves are not terribly sophisticated; I just test for
whether the goal has been reached and whether the separation goes
outside of the initial and goal values by more than max_separation_error.
Prints out something for a failure of either condition and returns
@@ -367,112 +109,76 @@
goal: goal state.
show_graph: Whether or not to display a graph showing the changing
states and voltages.
- iterations: Number of timesteps to run the model for."""
+ iterations: Number of timesteps to run the model for.
+ controller_claw: claw object to get K from, or None if we should
+ use claw.
+ observer_claw: claw object to use for the observer, or None if we should
+ use the actual state.
+ """
claw.X = initial_X
+ if controller_claw is None:
+ controller_claw = claw
+
+ if observer_claw is not None:
+ observer_claw.X_hat = initial_X + 0.01
+ observer_claw.X_hat = initial_X
+
# Various lists for graphing things.
t = []
- x_bottom = []
- x_top = []
- u_bottom = []
- u_top = []
- x_separation = []
+ x = []
+ v = []
+ x_hat = []
+ u = []
- tests_passed = True
-
- # Bounds which separation should not exceed.
- lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0]
- else goal[1, 0]) - max_separation_error
- upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0]
- else goal[1, 0]) + max_separation_error
+ sep_plot_gain = 100.0
for i in xrange(iterations):
- U = claw.K * (goal - claw.X)
- U = ScaleU(claw, U, claw.K, goal - claw.X)
+ X_hat = claw.X
+ if observer_claw is not None:
+ X_hat = observer_claw.X_hat
+ x_hat.append(observer_claw.X_hat[0, 0])
+ U = controller_claw.K * (goal - X_hat)
+ U[0, 0] = numpy.clip(U[0, 0], -12, 12)
+ x.append(claw.X[0, 0])
+ v.append(claw.X[1, 0])
+ if observer_claw is not None:
+ observer_claw.PredictObserver(U)
claw.Update(U)
-
- if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
- tests_passed = False
- print "Claw separation was", claw.X[1, 0]
- print "Should have been between", lower_bound, "and", upper_bound
-
- if claw.hard_pos_limits and \
- (claw.X[0, 0] > claw.hard_pos_limits[1] or
- claw.X[0, 0] < claw.hard_pos_limits[0] or
- claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
- claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
- tests_passed = False
- print "Claws at %f and %f" % (claw.X[0, 0], claw.X[0, 0] + claw.X[1, 0])
- print "Both should be in %s, definitely %s" % \
- (claw.pos_limits, claw.hard_pos_limits)
+ if observer_claw is not None:
+ observer_claw.Y = claw.Y
+ observer_claw.CorrectObserver(U)
t.append(i * claw.dt)
- x_bottom.append(claw.X[0, 0] * 10.0)
- x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
- u_bottom.append(U[0, 0])
- u_top.append(U[1, 0])
- x_separation.append(claw.X[1, 0] * 10.0)
+ u.append(U[0, 0])
if show_graph:
- pylab.plot(t, x_bottom, label='x bottom * 10')
- pylab.plot(t, x_top, label='x top * 10')
- pylab.plot(t, u_bottom, label='u bottom')
- pylab.plot(t, u_top, label='u top')
- pylab.plot(t, x_separation, label='separation * 10')
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x, label='x')
+ if observer_claw is not None:
+ pylab.plot(t, x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u, label='u')
pylab.legend()
pylab.show()
- # Test to make sure that we are near the goal.
- if numpy.max(abs(claw.X - goal)) > 1e-4:
- tests_passed = False
- print "X was", claw.X, "Expected", goal
-
- return tests_passed
def main(argv):
- claw = Claw()
+ loaded_mass = 5
+ #loaded_mass = 0
+ claw = Claw(mass=5 + loaded_mass)
+ claw_controller = Claw(mass=5 + 5)
+ observer_claw = Claw(mass=5 + 5)
+ #observer_claw = None
# Test moving the claw with constant separation.
- initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test just changing separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test changing both separation and position at once.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
-
- # Test opening with the top claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-1.6, 0.1)
- claw.pos_limits = (-1.5, 0.0)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
-
- # Test opening with the bottom claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-0.1, 1.6)
- claw.pos_limits = (0.0, 1.6)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0]])
+ run_test(claw, initial_X, R, controller_claw=claw_controller,
+ observer_claw=observer_claw)
# Write the generated constants out to a file.
if len(argv) != 3:
@@ -480,8 +186,6 @@
else:
claw = Claw("Claw")
loop_writer = control_loop.ControlLoopWriter("Claw", [claw])
- loop_writer.AddConstant(control_loop.Constant("kClawMomentOfInertiaRatio",
- "%f", claw.J_top / claw.J_bottom))
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index c3dd23e..ba4a43c 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -2,16 +2,16 @@
import numpy
class Constant(object):
- def __init__ (self, name, formatt, value):
- self.name = name
- self.formatt = formatt
- self.value = value
- self.formatToType = {}
- self.formatToType['%f'] = "double";
- self.formatToType['%d'] = "int";
- def __str__ (self):
- return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
- (self.formatToType[self.formatt], self.name, self.value)
+ def __init__ (self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double";
+ self.formatToType['%d'] = "int";
+ def __str__ (self):
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (self.formatToType[self.formatt], self.name, self.value)
class ControlLoopWriter(object):
@@ -140,24 +140,24 @@
fd.write('%s Make%sPlant() {\n' %
(self._PlantType(), self._gain_schedule_name))
- fd.write(' ::std::vector<%s *> plants(%d);\n' % (
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
self._CoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
- fd.write(' plants[%d] = new %s(%s);\n' %
- (index, self._CoeffType(),
+ fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._CoeffType(), self._CoeffType(),
loop.PlantFunction()))
- fd.write(' return %s(plants);\n' % self._PlantType())
+ fd.write(' return %s(&plants);\n' % self._PlantType())
fd.write('}\n\n')
fd.write('%s Make%sLoop() {\n' %
(self._LoopType(), self._gain_schedule_name))
- fd.write(' ::std::vector<%s *> controllers(%d);\n' % (
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
self._ControllerType(), len(self._loops)))
for index, loop in enumerate(self._loops):
- fd.write(' controllers[%d] = new %s(%s);\n' %
- (index, self._ControllerType(),
+ fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+ (index, self._ControllerType(), self._ControllerType(),
loop.ControllerFunction()))
- fd.write(' return %s(controllers);\n' % self._LoopType())
+ fd.write(' return %s(&controllers);\n' % self._LoopType())
fd.write('}\n\n')
fd.write(self._namespace_end)
@@ -216,6 +216,15 @@
self.X = self.A * self.X + self.B * U
self.Y = self.C * self.X + self.D * U
+ def PredictObserver(self, U):
+ """Runs the predict step of the observer update."""
+ self.X_hat = (self.A * self.X_hat + self.B * U)
+
+ def CorrectObserver(self, U):
+ """Runs the correct step of the observer update."""
+ self.X_hat += numpy.linalg.inv(self.A) * self.L * (
+ self.Y - self.C * self.X_hat - self.D * U)
+
def UpdateObserver(self, U):
"""Updates the observer given the provided U."""
self.X_hat = (self.A * self.X_hat + self.B * U +
@@ -320,9 +329,10 @@
ans.append(self._DumpMatrix('L', self.L))
ans.append(self._DumpMatrix('K', self.K))
+ ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
ans.append(' return StateFeedbackController<%d, %d, %d>'
- '(L, K, Make%sPlantCoefficients());\n' % (num_states, num_inputs,
- num_outputs, self._name))
+ '(L, K, A_inv, Make%sPlantCoefficients());\n' % (
+ num_states, num_inputs, num_outputs, self._name))
ans.append('}\n')
return ''.join(ans)
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 64bd52d..57268a9 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -89,8 +89,8 @@
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]))
+ diag = numpy.matrix(numpy.eye(A.shape[0]), dtype=numpy.complex128)
+ diage = numpy.matrix(numpy.eye(A.shape[0]), dtype=numpy.complex128)
for eig, count in zip(e, range(0, A.shape[0])):
diag[count, count] = numpy.exp(eig * dt)
if abs(eig) < 1.0e-16:
@@ -98,7 +98,13 @@
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)
+ ans_a = P * diag * numpy.linalg.inv(P)
+ ans_b = P * diage * numpy.linalg.inv(P) * B
+ if numpy.abs(ans_a.imag).sum() / numpy.abs(ans_a).sum() < 1e-6:
+ ans_a = ans_a.real
+ if numpy.abs(ans_b.imag).sum() / numpy.abs(ans_b).sum() < 1e-6:
+ ans_b = ans_b.real
+ return (ans_a, ans_b)
def ctrb(A, B):
"""Returns the controlability matrix.
@@ -123,7 +129,8 @@
# P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
- P, rcond, w, S, T = slycot.sb02od(A.shape[0], B.shape[1], A, B, Q, R, 'D')
+ P, rcond, w, S, T = slycot.sb02od(
+ n=A.shape[0], m=B.shape[1], A=A, B=B, Q=Q, R=R, dico='D')
F = numpy.linalg.inv(R + B.T * P *B) * B.T * P * A
return F
diff --git a/frc971/control_loops/python/elevator.py b/frc971/control_loops/python/elevator.py
new file mode 100755
index 0000000..2b42945
--- /dev/null
+++ b/frc971/control_loops/python/elevator.py
@@ -0,0 +1,246 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+import polytope
+import polydrivetrain
+import numpy
+import sys
+import matplotlib
+from matplotlib import pylab
+
+class Elevator(control_loop.ControlLoop):
+ def __init__(self, name="Elevator", mass=None):
+ super(Elevator, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.476
+ # Stall Current in Amps
+ self.stall_current = 80.730
+ # Free Speed in RPM
+ self.free_speed = 13906.0
+ # Free Current in Amps
+ self.free_current = 5.820
+ # Mass of the elevator
+ if mass is None:
+ self.mass = 13.0
+ else:
+ self.mass = mass
+
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = (56.0 / 12.0) * (84.0 / 14.0)
+ # Pulley diameter
+ self.r = 32 * 0.005 / numpy.pi / 2.0
+ # Control loop time step
+ self.dt = 0.005
+
+ # Elevator left/right spring constant (N/m)
+ self.spring = 3000.0
+
+ # State is [average position, average velocity,
+ # position difference/2, velocity difference/2]
+ # Input is [V1, V2]
+
+ C1 = self.spring / (self.mass * 0.5)
+ C2 = self.Kt * self.G / (self.mass * 0.5 * self.r * self.R)
+ C3 = self.G * self.G * self.Kt / (
+ self.R * self.r * self.r * self.mass * 0.5 * self.Kv)
+
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, -C3, 0, 0],
+ [0, 0, 0, 1],
+ [0, 0, -C1 * 2.0, -C3]])
+
+ print "Full speed is", C2 / C3 * 12.0
+
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [C2 / 2.0, C2 / 2.0],
+ [0, 0],
+ [C2 / 2.0, -C2 / 2.0]])
+
+ self.C = numpy.matrix([[1, 0, 1, 0],
+ [1, 0, -1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ print self.A
+
+ controlability = controls.ctrb(self.A, self.B);
+ print "Rank of augmented controlability matrix.", numpy.linalg.matrix_rank(
+ controlability)
+
+ q_pos = 0.02
+ q_vel = 0.400
+ q_pos_diff = 0.01
+ q_vel_diff = 0.45
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos_diff ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel_diff ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, 1.0 / (12.0 ** 2.0)]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ print self.K
+
+ print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = 0.20
+ self.ipl = 0.05
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+def CapU(U):
+ if U[0, 0] - U[1, 0] > 24:
+ return numpy.matrix([[12], [-12]])
+ elif U[0, 0] - U[1, 0] < -24:
+ return numpy.matrix([[-12], [12]])
+ else:
+ max_u = max(U[0, 0], U[1, 0])
+ min_u = min(U[0, 0], U[1, 0])
+ if max_u > 12:
+ return U - (max_u - 12)
+ if min_u < -12:
+ return U - (min_u + 12)
+ return U
+
+
+def run_test(elevator, initial_X, goal, max_separation_error=0.01,
+ show_graph=True, iterations=200, controller_elevator=None,
+ observer_elevator=None):
+ """Runs the elevator plant with an initial condition and goal.
+
+ The tests themselves are not terribly sophisticated; I just test for
+ whether the goal has been reached and whether the separation goes
+ outside of the initial and goal values by more than max_separation_error.
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ elevator: elevator object to use.
+ initial_X: starting state.
+ goal: goal state.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for.
+ controller_elevator: elevator object to get K from, or None if we should
+ use elevator.
+ observer_elevator: elevator object to use for the observer, or None if we
+ should use the actual state.
+ """
+
+ elevator.X = initial_X
+
+ if controller_elevator is None:
+ controller_elevator = elevator
+
+ if observer_elevator is not None:
+ observer_elevator.X_hat = initial_X + 0.01
+ observer_elevator.X_hat = initial_X
+
+ # Various lists for graphing things.
+ t = []
+ x_avg = []
+ x_sep = []
+ x_hat_avg = []
+ x_hat_sep = []
+ v_avg = []
+ v_sep = []
+ u_left = []
+ u_right = []
+
+ sep_plot_gain = 100.0
+
+ for i in xrange(iterations):
+ X_hat = elevator.X
+ if observer_elevator is not None:
+ X_hat = observer_elevator.X_hat
+ x_hat_avg.append(observer_elevator.X_hat[0, 0])
+ x_hat_sep.append(observer_elevator.X_hat[2, 0] * sep_plot_gain)
+ U = controller_elevator.K * (goal - X_hat)
+ U = CapU(U)
+ x_avg.append(elevator.X[0, 0])
+ v_avg.append(elevator.X[1, 0])
+ x_sep.append(elevator.X[2, 0] * sep_plot_gain)
+ v_sep.append(elevator.X[3, 0])
+ if observer_elevator is not None:
+ observer_elevator.PredictObserver(U)
+ elevator.Update(U)
+ if observer_elevator is not None:
+ observer_elevator.Y = elevator.Y
+ observer_elevator.CorrectObserver(U)
+
+ t.append(i * elevator.dt)
+ u_left.append(U[0, 0])
+ u_right.append(U[1, 0])
+
+ print numpy.linalg.inv(elevator.A)
+ print "delta time is ", elevator.dt
+ print "Velocity at t=0 is ", x_avg[0], v_avg[0], x_sep[0], v_sep[0]
+ print "Velocity at t=1+dt is ", x_avg[1], v_avg[1], x_sep[1], v_sep[1]
+
+ if show_graph:
+ pylab.subplot(2, 1, 1)
+ pylab.plot(t, x_avg, label='x avg')
+ pylab.plot(t, x_sep, label='x sep')
+ if observer_elevator is not None:
+ pylab.plot(t, x_hat_avg, label='x_hat avg')
+ pylab.plot(t, x_hat_sep, label='x_hat sep')
+ pylab.legend()
+
+ pylab.subplot(2, 1, 2)
+ pylab.plot(t, u_left, label='u left')
+ pylab.plot(t, u_right, label='u right')
+ pylab.legend()
+ pylab.show()
+
+
+def main(argv):
+ loaded_mass = 25
+ #loaded_mass = 0
+ elevator = Elevator(mass=13 + loaded_mass)
+ elevator_controller = Elevator(mass=13 + 15)
+ observer_elevator = Elevator(mass=13 + 15)
+ #observer_elevator = None
+
+ # Test moving the elevator with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.01], [0.0]])
+ #initial_X = numpy.matrix([[0.0], [0.0], [0.00], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(elevator, initial_X, R, controller_elevator=elevator_controller,
+ observer_elevator=observer_elevator)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name for the elevator."
+ else:
+ elevator = Elevator("Elevator")
+ loop_writer = control_loop.ControlLoopWriter("Elevator", [elevator])
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))