Added simulator/controller for third-robot elevator.
Change-Id: Id92ec8561aa47cdd17460efe91986e3876ed77eb
diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
new file mode 100644
index 0000000..881880f
--- /dev/null
+++ b/bot3/control_loops/python/control_loop.py
@@ -0,0 +1,338 @@
+import controls
+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)
+
+
+class ControlLoopWriter(object):
+ def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
+ """Constructs a control loop writer.
+
+ Args:
+ gain_schedule_name: string, Name of the overall controller.
+ loops: array[ControlLoop], a list of control loops to gain schedule
+ in order.
+ namespaces: array[string], a list of names of namespaces to nest in
+ order. If None, the default will be used.
+ """
+ self._gain_schedule_name = gain_schedule_name
+ self._loops = loops
+ if namespaces:
+ self._namespaces = namespaces
+ else:
+ self._namespaces = ['frc971', 'control_loops']
+
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
+
+ self._namespace_end = '\n'.join(
+ ['} // namespace %s' % name for name in reversed(self._namespaces)])
+
+ self._constant_list = []
+
+ def AddConstant(self, constant):
+ """Adds a constant to write.
+
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
+
+ def _TopDirectory(self):
+ return self._namespaces[0]
+
+ def _HeaderGuard(self, header_file):
+ return (self._TopDirectory().upper() + '_CONTROL_LOOPS_' +
+ header_file.upper().replace('.', '_').replace('/', '_') +
+ '_')
+
+ def Write(self, header_file, cc_file):
+ """Writes the loops to the specified files."""
+ self.WriteHeader(header_file)
+ self.WriteCC(header_file, cc_file)
+
+ def _GenericType(self, typename):
+ """Returns a loop template using typename for the type."""
+ num_states = self._loops[0].A.shape[0]
+ num_inputs = self._loops[0].B.shape[1]
+ num_outputs = self._loops[0].C.shape[0]
+ return '%s<%d, %d, %d>' % (
+ typename, num_states, num_inputs, num_outputs)
+
+ def _ControllerType(self):
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
+
+ def _LoopType(self):
+ """Returns a template name for StateFeedbackLoop."""
+ return self._GenericType('StateFeedbackLoop')
+
+ def _PlantType(self):
+ """Returns a template name for StateFeedbackPlant."""
+ return self._GenericType('StateFeedbackPlant')
+
+ def _CoeffType(self):
+ """Returns a template name for StateFeedbackPlantCoefficients."""
+ return self._GenericType('StateFeedbackPlantCoefficients')
+
+ def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
+ """Writes the header file to the file named header_file.
+ Set double_appendage to true in order to include a ratio of
+ moments of inertia constant. Currently, only used for 2014 claw."""
+ with open(header_file, 'w') as fd:
+ header_guard = self._HeaderGuard(header_file)
+ fd.write('#ifndef %s\n'
+ '#define %s\n\n' % (header_guard, header_guard))
+ fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ fd.write('\n')
+
+ fd.write(self._namespace_start)
+
+ for const in self._constant_list:
+ fd.write(str(const))
+
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(loop.DumpPlantHeader())
+ fd.write('\n')
+ fd.write(loop.DumpControllerHeader())
+ fd.write('\n')
+
+ fd.write('%s Make%sPlant();\n\n' %
+ (self._PlantType(), self._gain_schedule_name))
+
+ fd.write('%s Make%sLoop();\n\n' %
+ (self._LoopType(), self._gain_schedule_name))
+
+ fd.write(self._namespace_end)
+ fd.write('\n\n')
+ fd.write("#endif // %s\n" % header_guard)
+
+ def WriteCC(self, header_file_name, cc_file):
+ """Writes the cc file to the file named cc_file."""
+ with open(cc_file, 'w') as fd:
+ fd.write('#include \"%s/control_loops/%s\"\n' %
+ (self._TopDirectory(), header_file_name))
+ fd.write('\n')
+ fd.write('#include <vector>\n')
+ fd.write('\n')
+ fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+ fd.write('\n')
+ fd.write(self._namespace_start)
+ fd.write('\n\n')
+ for loop in self._loops:
+ fd.write(loop.DumpPlant())
+ fd.write('\n')
+
+ for loop in self._loops:
+ fd.write(loop.DumpController())
+ fd.write('\n')
+
+ fd.write('%s Make%sPlant() {\n' %
+ (self._PlantType(), self._gain_schedule_name))
+ 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] = ::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('}\n\n')
+
+ fd.write('%s Make%sLoop() {\n' %
+ (self._LoopType(), self._gain_schedule_name))
+ 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] = ::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('}\n\n')
+
+ fd.write(self._namespace_end)
+ fd.write('\n')
+
+
+class ControlLoop(object):
+ def __init__(self, name):
+ """Constructs a control loop object.
+
+ Args:
+ name: string, The name of the loop to use when writing the C++ files.
+ """
+ self._name = name
+
+ def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
+ """Calculates the discrete time values for A and B.
+
+ Args:
+ A_continuous: numpy.matrix, The continuous time A matrix
+ B_continuous: numpy.matrix, The continuous time B matrix
+ dt: float, The time step of the control loop
+
+ Returns:
+ (A, B), numpy.matrix, the control matricies.
+ """
+ return controls.c2d(A_continuous, B_continuous, dt)
+
+ def InitializeState(self):
+ """Sets X, Y, and X_hat to zero defaults."""
+ self.X = numpy.zeros((self.A.shape[0], 1))
+ self.Y = self.C * self.X
+ self.X_hat = numpy.zeros((self.A.shape[0], 1))
+
+ def PlaceControllerPoles(self, poles):
+ """Places the controller poles.
+
+ Args:
+ poles: array, An array of poles. Must be complex conjegates if they have
+ any imaginary portions.
+ """
+ self.K = controls.dplace(self.A, self.B, poles)
+
+ def PlaceObserverPoles(self, poles):
+ """Places the observer poles.
+
+ Args:
+ poles: array, An array of poles. Must be complex conjegates if they have
+ any imaginary portions.
+ """
+ self.L = controls.dplace(self.A.T, self.C.T, poles).T
+
+ def Update(self, U):
+ """Simulates one time step with the provided 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 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 +
+ self.L * (self.Y - self.C * self.X_hat - self.D * U))
+
+ def _DumpMatrix(self, matrix_name, matrix):
+ """Dumps the provided matrix into a variable called matrix_name.
+
+ Args:
+ matrix_name: string, The variable name to save the matrix to.
+ matrix: The matrix to dump.
+
+ Returns:
+ string, The C++ commands required to populate a variable named matrix_name
+ with the contents of matrix.
+ """
+ ans = [' Eigen::Matrix<double, %d, %d> %s;\n' % (
+ matrix.shape[0], matrix.shape[1], matrix_name)]
+ first = True
+ for x in xrange(matrix.shape[0]):
+ for y in xrange(matrix.shape[1]):
+ element = matrix[x, y]
+ 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):
+ """Writes out a c++ header declaration which will create a Plant object.
+
+ Returns:
+ string, The header declaration for the function.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
+ num_states, num_inputs, num_outputs, self._name)
+
+ def DumpPlant(self):
+ """Writes out a c++ function which will create a PlantCoefficients object.
+
+ Returns:
+ string, The function which will create the object.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
+ ' Make%sPlantCoefficients() {\n' % (
+ num_states, num_inputs, num_outputs, self._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 StateFeedbackPlantCoefficients<%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 PlantFunction(self):
+ """Returns the name of the plant coefficient function."""
+ return 'Make%sPlantCoefficients()' % self._name
+
+ def ControllerFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sController()' % self._name
+
+ def DumpControllerHeader(self):
+ """Writes out a c++ header declaration which will create a Controller object.
+
+ Returns:
+ string, The header declaration for the function.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
+ num_states, num_inputs, num_outputs, self.ControllerFunction())
+
+ def DumpController(self):
+ """Returns a c++ function which will create a Controller object.
+
+ Returns:
+ string, The function which will create the object.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
+ num_states, num_inputs, num_outputs, self.ControllerFunction())]
+
+ 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, A_inv, Make%sPlantCoefficients());\n' % (
+ num_states, num_inputs, num_outputs, self._name))
+ ans.append('}\n')
+ return ''.join(ans)
diff --git a/bot3/control_loops/python/controls.py b/bot3/control_loops/python/controls.py
new file mode 100644
index 0000000..b66bd56
--- /dev/null
+++ b/bot3/control_loops/python/controls.py
@@ -0,0 +1,153 @@
+#!/usr/bin/python
+
+"""
+Control loop pole placement library.
+
+This library will grow to support many different pole placement methods.
+Currently it only supports direct pole placement.
+"""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+import numpy
+import slycot
+import scipy.signal.cont2discrete
+
+class Error (Exception):
+ """Base class for all control loop exceptions."""
+
+
+class PolePlacementError(Error):
+ """Exception raised when pole placement fails."""
+
+
+# TODO(aschuh): dplace should take a control system object.
+# There should also exist a function to manipulate laplace expressions, and
+# something to plot bode plots and all that.
+def dplace(A, B, poles, alpha=1e-6):
+ """Set the poles of (A - BF) to poles.
+
+ Args:
+ A: numpy.matrix(n x n), The A matrix.
+ B: numpy.matrix(n x m), The B matrix.
+ poles: array(imaginary numbers), The poles to use. Complex conjugates poles
+ must be in pairs.
+
+ Raises:
+ ValueError: Arguments were the wrong shape or there were too many poles.
+ PolePlacementError: Pole placement failed.
+
+ Returns:
+ numpy.matrix(m x n), K
+ """
+ # See http://www.icm.tu-bs.de/NICONET/doc/SB01BD.html for a description of the
+ # fortran code that this is cleaning up the interface to.
+ n = A.shape[0]
+ if A.shape[1] != n:
+ raise ValueError("A must be square")
+ if B.shape[0] != n:
+ raise ValueError("B must have the same number of states as A.")
+ m = B.shape[1]
+
+ num_poles = len(poles)
+ if num_poles > n:
+ raise ValueError("Trying to place more poles than states.")
+
+ out = slycot.sb01bd(n=n,
+ m=m,
+ np=num_poles,
+ alpha=alpha,
+ A=A,
+ B=B,
+ w=numpy.array(poles),
+ dico='D')
+
+ A_z = numpy.matrix(out[0])
+ num_too_small_eigenvalues = out[2]
+ num_assigned_eigenvalues = out[3]
+ num_uncontrollable_eigenvalues = out[4]
+ K = numpy.matrix(-out[5])
+ Z = numpy.matrix(out[6])
+
+ if num_too_small_eigenvalues != 0:
+ raise PolePlacementError("Number of eigenvalues that are too small "
+ "and are therefore unmodified is %d." %
+ num_too_small_eigenvalues)
+ if num_assigned_eigenvalues != num_poles:
+ raise PolePlacementError("Did not place all the eigenvalues that were "
+ "requested. Only placed %d eigenvalues." %
+ num_assigned_eigenvalues)
+ if num_uncontrollable_eigenvalues != 0:
+ raise PolePlacementError("Found %d uncontrollable eigenvlaues." %
+ num_uncontrollable_eigenvalues)
+
+ return K
+
+
+def c2d(A, B, dt):
+ """Converts from continuous time state space representation to discrete time.
+ Returns (A, B). C and D are unchanged."""
+
+ ans_a, ans_b, _, _, _ = scipy.signal.cont2discrete((A, B, None, None), dt)
+ return numpy.matrix(ans_a), numpy.matrix(ans_b)
+
+def ctrb(A, B):
+ """Returns the controlability matrix.
+
+ This matrix must have full rank for all the states to be controlable.
+ """
+ n = A.shape[0]
+ output = B
+ intermediate = B
+ for i in xrange(0, n):
+ intermediate = A * intermediate
+ output = numpy.concatenate((output, intermediate), axis=1)
+
+ return output
+
+def dlqr(A, B, Q, R):
+ """Solves for the optimal lqr controller.
+
+ x(n+1) = A * x(n) + B * u(n)
+ J = sum(0, inf, x.T * Q * x + u.T * R * u)
+ """
+
+ # 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(
+ 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
+
+def kalman(A, B, C, Q, R):
+ """Solves for the steady state kalman gain and covariance matricies.
+
+ Args:
+ A, B, C: SS matricies.
+ Q: The model uncertantity
+ R: The measurement uncertainty
+
+ Returns:
+ KalmanGain, Covariance.
+ """
+ P = Q
+
+ I = numpy.matrix(numpy.eye(P.shape[0]))
+ At = A.T
+ Ct = C.T
+ i = 0
+
+ while True:
+ last_P = P
+ P_prior = A * P * At + Q
+ S = C * P_prior * Ct + R
+ K = P_prior * Ct * numpy.linalg.inv(S)
+ P = (I - K * C) * P_prior
+
+ diff = P - last_P
+ i += 1
+ if numpy.linalg.norm(diff) / numpy.linalg.norm(P) < 1e-9:
+ break
+
+ return K, P
diff --git a/bot3/control_loops/python/elevator3.py b/bot3/control_loops/python/elevator3.py
new file mode 100755
index 0000000..120e8cc
--- /dev/null
+++ b/bot3/control_loops/python/elevator3.py
@@ -0,0 +1,273 @@
+#!/usr/bin/python
+
+import control_loop
+import controls
+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 = 2.402
+ # Stall Current in Amps
+ self.stall_current = 126.145
+ # Free Speed in RPM
+ self.free_speed = 5015.562
+ # Free Current in Amps
+ self.free_current = 1.170
+ # Mass of the Elevator
+ if mass is None:
+ self.mass = 5.0
+ else:
+ self.mass = mass
+
+ # Number of motors
+ self.num_motors = 2.0
+ # Resistance of the motor
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = (self.num_motors * self.stall_torque) / self.stall_current
+ # Gear ratio
+ self.G = 8
+ # Radius of pulley
+ self.r = 0.0254
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # State is [position, velocity]
+ # Input is [Voltage]
+
+ C1 = self.Kt * self.G * self.G / (self.Kv * self.resistance * self.r * self.r * self.mass)
+ C2 = self.G * self.Kt / (self.resistance * self.r * self.mass)
+
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -C1]])
+
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [C2]])
+
+ 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)
+
+ controlability = controls.ctrb(self.A, self.B);
+
+ q_pos = 0.015
+ 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 'K', self.K
+ print 'Poles are', numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+ self.rpl = 0.30
+ self.ipl = 0.10
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ # print 'L is', self.L
+
+ q_pos = 0.05
+ q_vel = 2.65
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
+ [0.0, (q_vel ** 2.0)]])
+
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ # print 'Kal', self.KalmanGain
+ self.L = self.A * self.KalmanGain
+ print 'KalL is', self.L
+
+ # 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()
+
+class IntegralElevator(Elevator):
+ def __init__(self, name="IntegralElevator", mass=None):
+ super(IntegralElevator, self).__init__(name=name, mass=mass)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 0.08
+ q_vel = 0.40
+ q_voltage = 6.0
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0],
+ [0.0, 0.0, (q_voltage ** 2.0)]])
+
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+
+ self.InitializeState()
+
+
+class ScenarioPlotter(object):
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+
+ def run_test(self, elevator, goal,
+ iterations=200, controller_elevator=None,
+ observer_elevator=None):
+ """Runs the Elevator plant with an initial condition and goal.
+
+ Args:
+ Elevator: elevator object to use.
+ initial_X: starting state.
+ goal: goal state.
+ 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.
+ """
+
+ if controller_elevator is None:
+ controller_elevator = elevator
+
+ vbat = 10.0
+ if self.t:
+ initial_t = self.t[-1] + elevator.dt
+ else:
+ initial_t = 0
+ for i in xrange(iterations):
+ X_hat = elevator.X
+ if observer_elevator is not None:
+ X_hat = observer_elevator.X_hat
+ self.x_hat.append(observer_elevator.X_hat[0, 0])
+ gravity_compensation = 9.8 * elevator.mass * elevator.r / elevator.G / elevator.Kt * elevator.resistance
+
+ U = controller_elevator.K * (goal - X_hat)
+ U[0, 0] = numpy.clip(U[0, 0], -vbat , vbat )
+ self.x.append(elevator.X[0, 0])
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+ self.v.append(elevator.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / elevator.dt)
+
+ if observer_elevator is not None:
+ observer_elevator.Y = elevator.Y
+ observer_elevator.CorrectObserver(U)
+
+ elevator.Update(U - gravity_compensation)
+
+ if observer_elevator is not None:
+ observer_elevator.PredictObserver(U)
+
+ self.t.append(initial_t + i * elevator.dt)
+ self.u.append(U[0, 0])
+# if numpy.abs((goal - X_hat)[0:2, 0]).sum() < .025:
+# print "Time: ", self.t[-1]
+# break
+
+ print "Time: ", self.t[-1]
+
+
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+
+ pylab.legend()
+ pylab.show()
+
+
+def main(argv):
+ loaded_mass = 7+4.0
+ #loaded_mass = 0
+ #observer_elevator = None
+
+ # Test moving the Elevator
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ up_R = numpy.matrix([[0.4572], [0.0], [0.0]])
+ down_R = numpy.matrix([[0.0], [0.0], [0.0]])
+ totemass = 3.54
+ scenario_plotter = ScenarioPlotter()
+
+ elevator_controller = IntegralElevator(mass=4*totemass + loaded_mass)
+ observer_elevator = IntegralElevator(mass=4*totemass + loaded_mass)
+
+ for i in xrange(0, 7):
+ elevator = Elevator(mass=i*totemass + loaded_mass)
+ print 'Actual poles are', numpy.linalg.eig(elevator.A - elevator.B * elevator_controller.K[0, 0:2])[0]
+
+ elevator.X = initial_X
+ scenario_plotter.run_test(elevator, goal=up_R, controller_elevator=elevator_controller,
+ observer_elevator=observer_elevator, iterations=200)
+ scenario_plotter.run_test(elevator, goal=down_R, controller_elevator=elevator_controller,
+ observer_elevator=observer_elevator, iterations=200)
+
+ scenario_plotter.Plot()
+
+ # 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))