Moved bot3-specific python stuff in with bot3.
diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
new file mode 100644
index 0000000..832d4cc
--- /dev/null
+++ b/bot3/control_loops/python/control_loop.py
@@ -0,0 +1,297 @@
+import sys
+sys.path.append('../../frc971/control_loops/python')
+import controls
+import numpy
+
+class ControlLoopWriter(object):
+ def __init__(self, gain_schedule_name, loops, namespaces=None):
+ """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 = ['bot3', '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)])
+
+ def _HeaderGuard(self, header_file):
+ return ('BOT3_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):
+ """Writes the header file to the file named header_file."""
+ 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)
+ 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 \"bot3/control_loops/%s\"\n' % 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<%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(),
+ 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<%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(),
+ 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 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(' return StateFeedbackController<%d, %d, %d>'
+ '(L, K, Make%sPlantCoefficients());\n' % (num_states, num_inputs,
+ num_outputs, self._name))
+ ans.append('}\n')
+ return ''.join(ans)
diff --git a/bot3/control_loops/python/shooter.py b/bot3/control_loops/python/shooter.py
new file mode 100755
index 0000000..27ecc16
--- /dev/null
+++ b/bot3/control_loops/python/shooter.py
@@ -0,0 +1,137 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from matplotlib import pylab
+import control_loop
+import slycot
+
+class Shooter(control_loop.ControlLoop):
+ def __init__(self):
+ super(Shooter, self).__init__("Shooter")
+ # Stall Torque in N m
+ self.stall_torque = 2.42211227883219
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM
+ self.free_speed = 4650.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.0032
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ 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 = 40.0 / 34.0
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt)
+
+ self.InitializeState()
+
+ self.PlaceControllerPoles([.8])
+ # LQR stuff for optimization, if needed.
+ #print self.K
+ #self.R_LQR = numpy.matrix([[1.5]])
+ #self.P = slycot.sb02od(1, 1, self.A, self.B, self.C * self.C.T, self.R, 'D')[0]
+ #self.K = (numpy.linalg.inv(self.R_LQR + self.B.T * self.P * self.B)
+ # * self.B.T * self.P * self.A)
+ #print numpy.linalg.eig(self.A - self.B * self.K)
+
+
+ self.PlaceObserverPoles([0.45])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ shooter_data = numpy.genfromtxt('shooter/shooter_data.csv', delimiter=',')
+ shooter = Shooter()
+ simulated_x = []
+ real_x = []
+ x_vel = []
+ initial_x = shooter_data[0, 2]
+ last_x = initial_x
+ for i in xrange(shooter_data.shape[0]):
+ shooter.Update(numpy.matrix([[shooter_data[i, 1]]]))
+ simulated_x.append(shooter.X[0, 0])
+ x_offset = shooter_data[i, 2] - initial_x
+ real_x.append(x_offset)
+ x_vel.append((shooter_data[i, 2] - last_x) * 100.0)
+ last_x = shooter_data[i, 2]
+
+ sim_delay = 1
+# pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
+# simulated_x, label='Simulation')
+# pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
+# pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
+# pylab.legend()
+# pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ shooter = Shooter()
+ close_loop_x = []
+ close_loop_U = []
+ velocity_goal = 400
+ R = numpy.matrix([[velocity_goal]])
+ goal = False
+ for i in pylab.linspace(0,1.99,200):
+ # Iterate the position up.
+ R = numpy.matrix([[velocity_goal]])
+ U = numpy.clip(shooter.K * (R - shooter.X_hat) +
+ (numpy.identity(shooter.A.shape[0]) - shooter.A) * R / shooter.B,
+ shooter.U_min, shooter.U_max)
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ close_loop_x.append(shooter.X[0, 0])
+ close_loop_U.append(U[0, 0])
+ if (abs(R[0, 0] - shooter.X[0, 0]) < R[0, 0]* 0.01 and (not goal)):
+ goal = True
+ print i
+
+ #pylab.plotfile("shooter.csv", (0,1))
+ pylab.plot(pylab.linspace(0,1.99,200), close_loop_U)
+ #pylab.plotfile("shooter.csv", (0,2))
+ pylab.plot(pylab.linspace(0,1.99,200), close_loop_x)
+ pylab.show()
+
+ # Simulate spin down.
+ spin_down_x = [];
+ for _ in xrange(150):
+ U = 0
+ shooter.UpdateObserver(U)
+ shooter.Update(U)
+ spin_down_x.append(shooter.X[0, 0])
+
+ #pylab.plot(range(150), spin_down_x)
+ #pylab.show()
+
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter])
+ 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/bot3/control_loops/update_shooter.sh b/bot3/control_loops/update_shooter.sh
index 26e7ae3..db98547 100755
--- a/bot3/control_loops/update_shooter.sh
+++ b/bot3/control_loops/update_shooter.sh
@@ -2,4 +2,4 @@
#
# Updates the shooter controller.
-../../frc971/control_loops/python/shooter.py shooter/shooter_motor_plant.h shooter/shooter_motor_plant.cc
+./python/shooter.py shooter/shooter_motor_plant.h shooter/shooter_motor_plant.cc
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 9a8cac8..754ba62 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -17,7 +17,7 @@
if namespaces:
self._namespaces = namespaces
else:
- self._namespaces = ['bot3', 'control_loops']
+ self._namespaces = ['frc971', 'control_loops']
self._namespace_start = '\n'.join(
['namespace %s {' % name for name in self._namespaces])
@@ -26,7 +26,7 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
def _HeaderGuard(self, header_file):
- return ('BOT3_CONTROL_LOOPS_' +
+ return ('FRC971_CONTROL_LOOPS_' +
header_file.upper().replace('.', '_').replace('/', '_') +
'_')
@@ -89,7 +89,7 @@
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 \"bot3/control_loops/%s\"\n' % header_file_name)
+ fd.write('#include \"frc971/control_loops/%s\"\n' % header_file_name)
fd.write('\n')
fd.write('#include <vector>\n')
fd.write('\n')
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 27ecc16..83beb90 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -4,57 +4,51 @@
import sys
from matplotlib import pylab
import control_loop
-import slycot
class Shooter(control_loop.ControlLoop):
def __init__(self):
super(Shooter, self).__init__("Shooter")
# Stall Torque in N m
- self.stall_torque = 2.42211227883219
+ self.stall_torque = 0.49819248
# Stall Current in Amps
- self.stall_current = 133
+ self.stall_current = 85
# Free Speed in RPM
- self.free_speed = 4650.0
+ self.free_speed = 19300.0 - 1500.0
# Free Current in Amps
- self.free_current = 2.7
+ self.free_current = 1.4
# Moment of inertia of the shooter wheel in kg m^2
self.J = 0.0032
# Resistance of the motor, divided by 2 to account for the 2 motors
- self.R = 12.0 / self.stall_current
+ self.R = 12.0 / self.stall_current / 2
# 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 = 40.0 / 34.0
+ self.G = 11.0 / 34.0
# Control loop time step
self.dt = 0.01
# State feedback matrices
self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1]])
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ 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.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
- self.InitializeState()
+ self.PlaceControllerPoles([.6, .981])
- self.PlaceControllerPoles([.8])
- # LQR stuff for optimization, if needed.
- #print self.K
- #self.R_LQR = numpy.matrix([[1.5]])
- #self.P = slycot.sb02od(1, 1, self.A, self.B, self.C * self.C.T, self.R, 'D')[0]
- #self.K = (numpy.linalg.inv(self.R_LQR + self.B.T * self.P * self.B)
- # * self.B.T * self.P * self.A)
- #print numpy.linalg.eig(self.A - self.B * self.K)
-
-
- self.PlaceObserverPoles([0.45])
+ self.rpl = .45
+ self.ipl = 0.07
+ 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]])
@@ -78,47 +72,56 @@
last_x = shooter_data[i, 2]
sim_delay = 1
-# pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
-# simulated_x, label='Simulation')
-# pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
-# pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
-# pylab.legend()
-# pylab.show()
+ pylab.plot(range(sim_delay, shooter_data.shape[0] + sim_delay),
+ simulated_x, label='Simulation')
+ pylab.plot(range(shooter_data.shape[0]), real_x, label='Reality')
+ pylab.plot(range(shooter_data.shape[0]), x_vel, label='Velocity')
+ pylab.legend()
+ pylab.show()
# Simulate the closed loop response of the system to a step input.
shooter = Shooter()
close_loop_x = []
close_loop_U = []
- velocity_goal = 400
- R = numpy.matrix([[velocity_goal]])
- goal = False
- for i in pylab.linspace(0,1.99,200):
+ velocity_goal = 300
+ R = numpy.matrix([[0.0], [velocity_goal]])
+ for _ in pylab.linspace(0,1.99,200):
# Iterate the position up.
- R = numpy.matrix([[velocity_goal]])
- U = numpy.clip(shooter.K * (R - shooter.X_hat) +
- (numpy.identity(shooter.A.shape[0]) - shooter.A) * R / shooter.B,
+ R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
+ # Prevents the position goal from going beyond what is necessary.
+ velocity_weight_scalar = 0.35
+ max_reference = (
+ (shooter.U_max[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+ shooter.K[0, 0] +
+ shooter.X_hat[0, 0])
+ min_reference = (
+ (shooter.U_min[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+ shooter.K[0, 0] +
+ shooter.X_hat[0, 0])
+ R[0, 0] = numpy.clip(R[0, 0], min_reference, max_reference)
+ U = numpy.clip(shooter.K * (R - shooter.X_hat),
shooter.U_min, shooter.U_max)
shooter.UpdateObserver(U)
shooter.Update(U)
- close_loop_x.append(shooter.X[0, 0])
+ close_loop_x.append(shooter.X[1, 0])
close_loop_U.append(U[0, 0])
- if (abs(R[0, 0] - shooter.X[0, 0]) < R[0, 0]* 0.01 and (not goal)):
- goal = True
- print i
#pylab.plotfile("shooter.csv", (0,1))
- pylab.plot(pylab.linspace(0,1.99,200), close_loop_U)
+ #pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
#pylab.plotfile("shooter.csv", (0,2))
- pylab.plot(pylab.linspace(0,1.99,200), close_loop_x)
+ pylab.plot(pylab.linspace(0,1.99,200), close_loop_x, 'ro')
pylab.show()
# Simulate spin down.
spin_down_x = [];
+ R = numpy.matrix([[50.0], [0.0]])
for _ in xrange(150):
U = 0
shooter.UpdateObserver(U)
shooter.Update(U)
- spin_down_x.append(shooter.X[0, 0])
+ spin_down_x.append(shooter.X[1, 0])
#pylab.plot(range(150), spin_down_x)
#pylab.show()