Added stuff to make shooter work.
Doesn't seem to start running when deployed to the robot.
diff --git a/bot3/atom_code/atom_code.gyp b/bot3/atom_code/atom_code.gyp
index 1b1ba1e..245e212 100644
--- a/bot3/atom_code/atom_code.gyp
+++ b/bot3/atom_code/atom_code.gyp
@@ -7,8 +7,8 @@
'<(AOS)/build/aos_all.gyp:Atom',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
- #'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_lib_test',
- #'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_lib_test',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter',
'<(DEPTH)/bot3/autonomous/autonomous.gyp:auto',
'<(DEPTH)/bot3/input/input.gyp:joystick_reader',
#'../input/input.gyp:AutoMode',
diff --git a/bot3/atom_code/scripts/start_list.txt b/bot3/atom_code/scripts/start_list.txt
index 5af6a46..46ece83 100644
--- a/bot3/atom_code/scripts/start_list.txt
+++ b/bot3/atom_code/scripts/start_list.txt
@@ -4,4 +4,5 @@
drivetrain
CRIOLogReader
auto
+shooter
gyro_sensor_receiver
diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
new file mode 100644
index 0000000..754ba62
--- /dev/null
+++ b/bot3/control_loops/python/control_loop.py
@@ -0,0 +1,295 @@
+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 = ['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)])
+
+ def _HeaderGuard(self, header_file):
+ return ('FRC971_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 \"frc971/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/controls.py b/bot3/control_loops/python/controls.py
new file mode 100644
index 0000000..a40bfe2
--- /dev/null
+++ b/bot3/control_loops/python/controls.py
@@ -0,0 +1,101 @@
+#!/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
+
+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.
+ 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/bot3/control_loops/python/drivetrain.py b/bot3/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..0e791cb
--- /dev/null
+++ b/bot3/control_loops/python/drivetrain.py
@@ -0,0 +1,162 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Drivetrain(control_loop.ControlLoop):
+ def __init__(self):
+ super(Drivetrain, self).__init__("Drivetrain")
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM. Used number from last year.
+ self.free_speed = 4650.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the drivetrain in kg m^2
+ # Just borrowed from last year.
+ self.J = 6.4
+ # Mass of the robot, in kg.
+ self.m = 68
+ # Radius of the robot, in meters (from last year).
+ self.rb = 0.617998644 / 2.0
+ # Radius of the wheels, in meters.
+ self.r = .04445
+ # Resistance of the motor, divided by the number of motors.
+ self.R = 12.0 / self.stall_current / 6 + 0.03
+ # 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 ratios
+ self.G_low = 16.0 / 60.0 * 19.0 / 50.0
+ self.G_high = 28.0 / 48.0 * 19.0 / 50.0
+ self.G = self.G_low
+ # Control loop time step
+ self.dt = 0.01
+
+ # These describe the way that a given side of a robot will be influenced
+ # by the other side. Units of 1 / kg.
+ self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+ self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+ # The calculations which we will need for A and B.
+ self.tc = -self.Kt / self.Kv / (self.G * self.G * self.R * self.r * self.r)
+ self.mp = self.Kt / (self.G * self.R * self.r)
+
+ # State feedback matrices
+ # X will be of the format
+ # [[position1], [velocity1], [position2], velocity2]]
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, self.msp * self.tc, 0, self.msn * self.tc],
+ [0, 0, 0, 1],
+ [0, self.msn * self.tc, 0, self.msp * self.tc]])
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [self.msp * self.mp, self.msn * self.mp],
+ [0, 0],
+ [self.msn * self.mp, self.msp * self.mp]])
+ self.C = numpy.matrix([[1, 0, 0, 0],
+ [0, 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)
+
+ # Poles from last year.
+ self.hp = 0.65
+ self.lp = 0.83
+ self.PlaceControllerPoles([self.hp, self.hp, self.lp, self.lp])
+
+ print self.K
+
+ self.hlp = 0.07
+ self.llp = 0.09
+ self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.InitializeState()
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ drivetrain = Drivetrain()
+ simulated_left = []
+ simulated_right = []
+ for _ in xrange(100):
+ drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+ simulated_left.append(drivetrain.X[0, 0])
+ simulated_right.append(drivetrain.X[2, 0])
+
+ #pylab.plot(range(100), simulated_left)
+ #pylab.plot(range(100), simulated_right)
+ #pylab.show()
+
+ # Simulate forwards motion.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ #pylab.plot(range(100), close_loop_left)
+ #pylab.plot(range(100), close_loop_right)
+ #pylab.show()
+
+ # Try turning in place
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ #pylab.plot(range(100), close_loop_left)
+ #pylab.plot(range(100), close_loop_right)
+ #pylab.show()
+
+ # Try turning just one side.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ #pylab.plot(range(100), close_loop_left)
+ #pylab.plot(range(100), close_loop_right)
+ #pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ loop_writer = control_loop.ControlLoopWriter("Drivetrain", [drivetrain])
+ 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/python/libcdd.py b/bot3/control_loops/python/libcdd.py
new file mode 100644
index 0000000..a217728
--- /dev/null
+++ b/bot3/control_loops/python/libcdd.py
@@ -0,0 +1,129 @@
+#!/usr/bin/python
+
+"""Wrapper around libcdd, a polytope manipulation library."""
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+import ctypes
+
+# Wrapper around PyFile_AsFile so that we can print out the error messages.
+# Set the arg type and return types of the function call.
+class FILE(ctypes.Structure):
+ pass
+
+ctypes.pythonapi.PyFile_AsFile.argtypes = [ctypes.py_object]
+ctypes.pythonapi.PyFile_AsFile.restype = ctypes.POINTER(FILE)
+
+# Load and init libcdd. libcdd is a C library that implements algorithm to
+# manipulate half space and vertex representations of polytopes.
+# Unfortunately, the library was compiled with C++ even though it has a lot of C
+# code in it, so all the symbol names are mangled. Ug.
+libcdd = ctypes.cdll.LoadLibrary('libcdd.so')
+libcdd._Z23dd_set_global_constantsv()
+
+# The variable type mytype that libcdd defines (double[1])
+# See http://docs.python.org/2/library/ctypes.html#arrays for the documentation
+# explaining why ctypes.c_double * 1 => double[1]
+# libcdd defines mytype to various things so it can essentially template its
+# functions. What a weird library.
+mytype = ctypes.c_double * 1
+
+
+# Forward declaration for the polyhedra data structure.
+class dd_polyhedradata(ctypes.Structure):
+ pass
+
+
+# Definition of dd_matrixdata
+class dd_matrixdata(ctypes.Structure):
+ _fields_ = [
+ ("rowsize", ctypes.c_long),
+ ("linset", ctypes.POINTER(ctypes.c_ulong)),
+ ("colsize", ctypes.c_long),
+ ("representation", ctypes.c_int),
+ ("numbtype", ctypes.c_int),
+ ("matrix", ctypes.POINTER(ctypes.POINTER(mytype))),
+ ("objective", ctypes.c_int),
+ ("rowvec", ctypes.POINTER(mytype)),
+ ]
+
+# Define the input and output types for a bunch of libcdd functions.
+libcdd._Z15dd_CreateMatrixll.restype = ctypes.POINTER(dd_matrixdata)
+libcdd._Z9ddd_get_dPd.argtypes = [mytype]
+libcdd._Z9ddd_get_dPd.restype = ctypes.c_double
+
+libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata.argtypes = [
+ ctypes.POINTER(dd_polyhedradata)
+]
+libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata.restype = ctypes.POINTER(dd_matrixdata)
+
+libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType.argtypes = [
+ ctypes.POINTER(dd_matrixdata),
+ ctypes.POINTER(ctypes.c_int)
+]
+libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType.restype = (
+ ctypes.POINTER(dd_polyhedradata))
+
+libcdd._Z13dd_FreeMatrixP13dd_matrixdata.argtypes = [
+ ctypes.POINTER(dd_matrixdata)
+]
+
+libcdd._Z16dd_FreePolyhedraP16dd_polyhedradata.argtypes = [
+ ctypes.POINTER(dd_polyhedradata)
+]
+
+libcdd._Z9ddd_set_dPdd.argtypes = [
+ mytype,
+ ctypes.c_double
+]
+
+
+# Various enums.
+DD_INEQUALITY = 1
+DD_REAL = 1
+DD_NO_ERRORS = 17
+
+
+def dd_CreateMatrix(rows, cols):
+ return libcdd._Z15dd_CreateMatrixll(
+ ctypes.c_long(rows),
+ ctypes.c_long(cols))
+
+
+def dd_set_d(mytype_address, double_value):
+ libcdd._Z9ddd_set_dPdd(mytype_address,
+ ctypes.c_double(double_value))
+
+
+def dd_CopyGenerators(polyhedraptr):
+ return libcdd._Z17dd_CopyGeneratorsP16dd_polyhedradata(polyhedraptr)
+
+
+def dd_get_d(mytype_address):
+ return libcdd._Z9ddd_get_dPd(mytype_address)
+
+
+def dd_FreeMatrix(matrixptr):
+ libcdd._Z13dd_FreeMatrixP13dd_matrixdata(matrixptr)
+
+
+def dd_FreePolyhedra(polyhedraptr):
+ libcdd._Z16dd_FreePolyhedraP16dd_polyhedradata(polyhedraptr)
+
+
+def dd_DDMatrix2Poly(matrixptr):
+ error = ctypes.c_int()
+ polyhedraptr = libcdd._Z16dd_DDMatrix2PolyP13dd_matrixdataP12dd_ErrorType(
+ matrixptr,
+ ctypes.byref(error))
+
+ # Return None on error.
+ # The error values are enums, so they aren't exposed.
+ if error.value != NO_ERRORS:
+ # Dump out the errors to stderr
+ libcdd._Z21dd_WriteErrorMessagesP8_IO_FILE12dd_ErrorType(
+ ctypes.pythonapi.PyFile_AsFile(ctypes.py_object(sys.stdout)),
+ error)
+ dd_FreePolyhedra(polyhedraptr)
+ return None
+ return polyhedraptr
diff --git a/bot3/control_loops/python/shooter.py b/bot3/control_loops/python/shooter.py
new file mode 100755
index 0000000..7cbe617
--- /dev/null
+++ b/bot3/control_loops/python/shooter.py
@@ -0,0 +1,140 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from matplotlib import pylab
+import control_loop
+
+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(
+ [[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.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.6, .981])
+
+ 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]])
+
+
+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 = 300
+ R = numpy.matrix([[0.0], [velocity_goal]])
+ for _ in pylab.linspace(0,1.99,200):
+ # Iterate the position up.
+ 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[1, 0])
+ close_loop_U.append(U[0, 0])
+
+ #pylab.plotfile("shooter.csv", (0,1))
+ #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, '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[1, 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/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..fbfeafb
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -0,0 +1,118 @@
+#include "bot3/control_loops/shooter/shooter.h"
+
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
+ : aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
+ loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
+ history_position_(0),
+ position_goal_(0.0),
+ last_position_(0.0),
+ last_velocity_goal_(0) {
+ memset(history_, 0, sizeof(history_));
+}
+
+/*static*/ const double ShooterMotor::dt = 0.01;
+
+void ShooterMotor::RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Status *status) {
+ double velocity_goal = goal->velocity;
+ const double current_position =
+ (position == NULL ? loop_->X_hat(0, 0) : position->position);
+ double output_voltage = 0.0;
+
+/* if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ if (index_loop.status->is_shooting) {
+ if (velocity_goal != last_velocity_goal_ &&
+ velocity_goal < 130) {
+ velocity_goal = last_velocity_goal_;
+ }
+ }
+ } else {
+ LOG(WARNING, "assuming index isn't shooting\n");
+ }*/
+ last_velocity_goal_ = velocity_goal;
+
+ // Track the current position if the velocity goal is small.
+ if (velocity_goal <= 1.0) {
+ position_goal_ = current_position;
+ }
+
+ loop_->Y << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ // Prevents integral windup by limiting the position error such that the
+ // error can't produce much more than full power.
+ const double kVelocityWeightScalar = 0.35;
+ const double max_reference =
+ (loop_->U_max(0, 0) - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
+ / loop_->K(0, 0) + loop_->X_hat(0, 0);
+ const double min_reference =
+ (loop_->U_min(0, 0) - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
+ / loop_->K(0, 0) + loop_->X_hat(0, 0);
+
+ position_goal_ = ::std::max(::std::min(position_goal_, max_reference),
+ min_reference);
+ loop_->R << position_goal_, velocity_goal;
+ position_goal_ += velocity_goal * dt;
+
+ loop_->Update(position, output == NULL);
+
+ // Kill power at low velocity goals.
+ if (velocity_goal < 1.0) {
+ loop_->U[0] = 0.0;
+ } else {
+ output_voltage = loop_->U[0];
+ }
+
+ LOG(DEBUG,
+ "PWM: %f, raw_pos: %f rotations: %f "
+ "junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
+ output_voltage, current_position,
+ current_position / (2 * M_PI),
+ (current_position - last_position_) / dt,
+ loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
+
+ // Calculates the velocity over the last kHistoryLength * .01 seconds
+ // by taking the difference between the current and next history positions.
+ int old_history_position = ((history_position_ == 0) ?
+ kHistoryLength : history_position_) - 1;
+ average_velocity_ = (history_[old_history_position] -
+ history_[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+
+ status->average_velocity = average_velocity_;
+
+ // Determine if the velocity is close enough to the goal to be ready.
+ if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
+ velocity_goal != 0.0) {
+ LOG(DEBUG, "Steady: ");
+ status->ready = true;
+ } else {
+ LOG(DEBUG, "Not ready: ");
+ status->ready = false;
+ }
+ LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
+
+ last_position_ = current_position;
+
+ if (output) {
+ output->voltage = output_voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter.gyp b/bot3/control_loops/shooter/shooter.gyp
new file mode 100644
index 0000000..ed42647
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.gyp
@@ -0,0 +1,79 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'shooter_loop',
+ 'type': 'static_library',
+ 'sources': ['shooter_motor.q'],
+ 'variables': {
+ 'header_path': 'bot3/control_loops/shooter',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:control_loop_queues',
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
+ 'target_name': 'shooter_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shooter.cc',
+ 'shooter_motor_plant.cc',
+ ],
+ 'dependencies': [
+ 'shooter_loop',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ #'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/common/common.gyp:controls',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_lib_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'shooter_loop',
+ 'shooter_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter_csv',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_csv.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ 'shooter_loop',
+ ],
+ },
+ {
+ 'target_name': 'shooter',
+ 'type': 'executable',
+ 'sources': [
+ 'shooter_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ 'shooter_lib',
+ 'shooter_loop',
+ ],
+ },
+ ],
+}
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..6ab1ea5
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter.h
@@ -0,0 +1,57 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+
+namespace bot3 {
+namespace control_loops {
+
+class ShooterMotor
+ : public aos::control_loops::ControlLoop<control_loops::ShooterLoop> {
+ public:
+ explicit ShooterMotor(
+ control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+
+ // Control loop time step.
+ static const double dt;
+
+ // Maximum speed of the shooter wheel which the encoder is rated for in
+ // rad/sec.
+ static const double kMaxSpeed;
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::ShooterLoop::Goal *goal,
+ const control_loops::ShooterLoop::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterLoop::Status *status);
+
+ private:
+ // The state feedback control loop to talk to.
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+
+ // History array and stuff for determining average velocity and whether
+ // we are ready to shoot.
+ static const int kHistoryLength = 5;
+ double history_[kHistoryLength];
+ ptrdiff_t history_position_;
+ double average_velocity_;
+
+ double position_goal_;
+ double last_position_;
+
+ // For making sure it keeps spinning if we're shooting.
+ double last_velocity_goal_;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
+};
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_H_
diff --git a/bot3/control_loops/shooter/shooter_csv.cc b/bot3/control_loops/shooter/shooter_csv.cc
new file mode 100644
index 0000000..c929f7e
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_csv.cc
@@ -0,0 +1,50 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+
+using ::bot3::control_loops::shooter;
+using ::aos::time::Time;
+
+int main(int argc, char * argv[]) {
+ FILE *data_file = NULL;
+ FILE *output_file = NULL;
+
+ if (argc == 2) {
+ data_file = fopen(argv[1], "w");
+ output_file = data_file;
+ } else {
+ printf("Logging to stdout instead\n");
+ output_file = stdout;
+ }
+
+ fprintf(data_file, "time, power, position");
+
+ ::aos::Init();
+
+ Time start_time = Time::Now();
+
+ while (true) {
+ ::aos::time::PhasedLoop10MS(2000);
+ shooter.goal.FetchLatest();
+ shooter.status.FetchLatest();
+ shooter.position.FetchLatest();
+ shooter.output.FetchLatest();
+ if (shooter.output.get() &&
+ shooter.position.get()) {
+ fprintf(output_file, "\n%f, %f, %f",
+ (shooter.position->sent_time - start_time).ToSeconds(),
+ shooter.output->voltage,
+ shooter.position->position);
+ }
+ }
+
+ if (data_file) {
+ fclose(data_file);
+ }
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/bot3/control_loops/shooter/shooter_data.csv b/bot3/control_loops/shooter/shooter_data.csv
new file mode 100644
index 0000000..3515070
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_data.csv
@@ -0,0 +1,638 @@
+0.009404, 0.000000, 1484.878965
+0.019423, 0.000000, 1484.878965
+0.029389, 0.000000, 1484.878965
+0.039354, 0.000000, 1484.878965
+0.049887, 0.000000, 1484.878965
+0.059522, 0.000000, 1484.878965
+0.069479, 0.000000, 1484.878965
+0.079381, 0.000000, 1484.878965
+0.089338, 0.000000, 1484.878965
+0.099357, 0.000000, 1484.878965
+0.109409, 0.000000, 1484.878965
+0.119355, 0.000000, 1484.878965
+0.129358, 0.000000, 1484.878965
+0.139354, 0.000000, 1484.878965
+0.149480, 0.000000, 1484.878965
+0.159363, 0.000000, 1484.878965
+0.169341, 0.000000, 1484.878965
+0.179367, 0.000000, 1484.878965
+0.189636, 0.000000, 1484.878965
+0.199875, 0.000000, 1484.878965
+0.210070, 0.000000, 1484.878965
+0.219349, 0.000000, 1484.878965
+0.229544, 0.000000, 1484.878965
+0.239404, 0.000000, 1484.878965
+0.249410, 0.000000, 1484.878965
+0.259839, 0.000000, 1484.878965
+0.269492, 0.000000, 1484.878965
+0.279847, 0.000000, 1484.878965
+0.290056, 0.000000, 1484.878965
+0.299362, 0.000000, 1484.878965
+0.309457, 0.000000, 1484.878965
+0.319829, 0.000000, 1484.878965
+0.329446, 0.000000, 1484.878965
+0.339818, 0.000000, 1484.878965
+0.349444, 0.000000, 1484.878965
+0.359899, 0.000000, 1484.878965
+0.370053, 0.000000, 1484.878965
+0.379510, 0.000000, 1484.878965
+0.390136, 0.000000, 1484.878965
+0.399366, 0.000000, 1484.878965
+0.409472, 0.000000, 1484.878965
+0.419898, 0.000000, 1484.878965
+0.430131, 0.000000, 1484.878965
+0.439363, 0.000000, 1484.878965
+0.449459, 0.000000, 1484.878965
+0.459840, 0.000000, 1484.878965
+0.469382, 0.000000, 1484.878965
+0.479846, 0.000000, 1484.878965
+0.489432, 0.000000, 1484.878965
+0.499342, 0.000000, 1484.878965
+0.509350, 0.000000, 1484.878965
+0.519406, 0.000000, 1484.878965
+0.530084, 0.000000, 1484.878965
+0.539341, 0.000000, 1484.878965
+0.549406, 0.000000, 1484.878965
+0.559401, 0.000000, 1484.878965
+0.569409, 0.000000, 1484.878965
+0.579831, 0.000000, 1484.878965
+0.589469, 0.000000, 1484.878965
+0.599356, 0.000000, 1484.878965
+0.610099, 0.000000, 1484.878965
+0.619333, 0.000000, 1484.878965
+0.629479, 0.000000, 1484.878965
+0.639805, 0.000000, 1484.878965
+0.650053, 0.000000, 1484.878965
+0.659423, 0.000000, 1484.878965
+0.669413, 0.000000, 1484.878965
+0.679822, 0.000000, 1484.878965
+0.690045, 0.000000, 1484.878965
+0.699411, 0.000000, 1484.878965
+0.709584, 0.000000, 1484.878965
+0.719866, 0.000000, 1484.878965
+0.729475, 0.000000, 1484.878965
+0.739328, 0.000000, 1484.878965
+0.749396, 0.000000, 1484.878965
+0.759836, 0.000000, 1484.878965
+0.769492, 0.000000, 1484.878965
+0.779340, 0.000000, 1484.878965
+0.789401, 0.000000, 1484.878965
+0.799405, 0.000000, 1484.878965
+0.809407, 0.000000, 1484.878965
+0.819830, 0.000000, 1484.878965
+0.829411, 0.000000, 1484.878965
+0.839413, 0.000000, 1484.878965
+0.849409, 0.000000, 1484.878965
+0.859349, 0.000000, 1484.878965
+0.869453, 0.000000, 1484.878965
+0.879831, 0.000000, 1484.878965
+0.889426, 0.000000, 1484.878965
+0.899332, 0.000000, 1484.878965
+0.909439, 0.000000, 1484.878965
+0.919830, 0.000000, 1484.878965
+0.929464, 0.000000, 1484.878965
+0.939328, 0.000000, 1484.878965
+0.949440, 0.000000, 1484.878965
+0.959871, 0.000000, 1484.878965
+0.969393, 0.000000, 1484.878965
+0.980080, 0.000000, 1484.878965
+0.989440, 0.000000, 1484.878965
+0.999370, 0.000000, 1484.878965
+1.009338, 0.000000, 1484.878965
+1.019368, 0.000000, 1484.878965
+1.029492, 0.000000, 1484.878965
+1.039816, 0.000000, 1484.878965
+1.049430, 0.000000, 1484.878965
+1.059324, 0.000000, 1484.878965
+1.069351, 0.000000, 1484.878965
+1.079867, 0.000000, 1484.878965
+1.089417, 0.000000, 1484.878965
+1.099324, 0.000000, 1484.878965
+1.109348, 0.000000, 1484.878965
+1.119389, 0.000000, 1484.878965
+1.129331, 0.000000, 1484.878965
+1.139306, 0.000000, 1484.878965
+1.149394, 0.000000, 1484.878965
+1.159374, 0.000000, 1484.878965
+1.169335, 0.000000, 1484.878965
+1.179817, 0.000000, 1484.878965
+1.189415, 0.000000, 1484.878965
+1.199338, 0.000000, 1484.878965
+1.209349, 0.000000, 1484.878965
+1.219333, 0.000000, 1484.878965
+1.229518, 0.000000, 1484.878965
+1.239329, 0.000000, 1484.878965
+1.249334, 0.000000, 1484.878965
+1.259316, 0.000000, 1484.878965
+1.269388, 0.000000, 1484.878965
+1.279357, 0.000000, 1484.878965
+1.289451, 0.000000, 1484.878965
+1.299350, 0.000000, 1484.878965
+1.309350, 0.000000, 1484.878965
+1.319848, 0.000000, 1484.878965
+1.329384, 0.000000, 1484.878965
+1.339375, 0.000000, 1484.878965
+1.349359, 0.000000, 1484.878965
+1.359384, 0.000000, 1484.878965
+1.369428, 0.000000, 1484.878965
+1.379443, 0.000000, 1484.878965
+1.389498, 0.000000, 1484.878965
+1.399332, 0.000000, 1484.878965
+1.409393, 0.000000, 1484.878965
+1.419325, 0.000000, 1484.878965
+1.430129, 0.000000, 1484.878965
+1.439419, 0.000000, 1484.878965
+1.449510, 0.000000, 1484.878965
+1.459828, 0.000000, 1484.878965
+1.469377, 0.000000, 1484.878965
+1.479834, 0.000000, 1484.878965
+1.489367, 0.000000, 1484.878965
+1.499316, 0.000000, 1484.878965
+1.509405, 0.000000, 1484.878965
+1.519341, 0.000000, 1484.878965
+1.529334, 0.000000, 1484.878965
+1.539305, 0.000000, 1484.878965
+1.550118, 0.000000, 1484.878965
+1.559386, 0.000000, 1484.878965
+1.569647, 0.000000, 1484.878965
+1.579395, 0.000000, 1484.878965
+1.589381, 0.000000, 1484.878965
+1.599819, 0.000000, 1484.878965
+1.609401, 0.000000, 1484.878965
+1.619404, 0.000000, 1484.878965
+1.629335, 0.000000, 1484.878965
+1.639327, 0.000000, 1484.878965
+1.649334, 0.000000, 1484.878965
+1.659341, 0.000000, 1484.878965
+1.669328, 0.000000, 1484.878965
+1.679850, 0.000000, 1484.878965
+1.689423, 0.000000, 1484.878965
+1.699320, 0.000000, 1484.878965
+1.710128, 0.000000, 1484.878965
+1.719388, 0.000000, 1484.878965
+1.730042, 0.000000, 1484.878965
+1.739338, 0.000000, 1484.878965
+1.749483, 0.000000, 1484.878965
+1.759420, 0.000000, 1484.878965
+1.769334, 0.000000, 1484.878965
+1.779289, 0.000000, 1484.878965
+1.789325, 0.000000, 1484.878965
+1.799395, 0.000000, 1484.878965
+1.809493, 0.000000, 1484.878965
+1.819312, 0.000000, 1484.878965
+1.829402, 0.000000, 1484.878965
+1.839317, 0.000000, 1484.878965
+1.849330, 0.000000, 1484.878965
+1.859354, 0.000000, 1484.878965
+1.869394, 0.000000, 1484.878965
+1.879816, 0.000000, 1484.878965
+1.889374, 0.000000, 1484.878965
+1.899381, 0.000000, 1484.878965
+1.909332, 0.000000, 1484.878965
+1.919359, 0.000000, 1484.878965
+1.929338, 0.000000, 1484.878965
+1.939359, 0.000000, 1484.878965
+1.949332, 0.000000, 1484.878965
+1.959325, 0.000000, 1484.878965
+1.969341, 0.000000, 1484.878965
+1.979362, 0.000000, 1484.878965
+1.989330, 0.000000, 1484.878965
+1.999479, 0.000000, 1484.878965
+2.009392, 0.000000, 1484.878965
+2.019318, 0.000000, 1484.878965
+2.029320, 0.000000, 1484.878965
+2.039323, 0.000000, 1484.878965
+2.049387, 0.000000, 1484.878965
+2.059818, 0.000000, 1484.878965
+2.069766, 0.000000, 1484.878965
+2.079835, 0.000000, 1484.878965
+2.089372, 0.000000, 1484.878965
+2.099322, 0.000000, 1484.878965
+2.109357, 0.000000, 1484.878965
+2.119387, 0.000000, 1484.878965
+2.129327, 0.000000, 1484.878965
+2.139458, 0.000000, 1484.878965
+2.149392, 0.000000, 1484.878965
+2.159826, 0.000000, 1484.878965
+2.169591, 0.000000, 1484.878965
+2.179656, 0.000000, 1484.878965
+2.189392, 0.000000, 1484.878965
+2.199491, 0.000000, 1484.878965
+2.209541, 0.000000, 1484.878965
+2.219287, 0.000000, 1484.878965
+2.229123, 0.000000, 1484.878965
+2.239347, 0.000000, 1484.878965
+2.249390, 0.000000, 1484.878965
+2.259407, 0.000000, 1484.878965
+2.269393, 0.000000, 1484.878965
+2.279375, 0.000000, 1484.878965
+2.289416, 0.000000, 1484.878965
+2.299368, 0.000000, 1484.878965
+2.309379, 0.000000, 1484.878965
+2.319382, 0.000000, 1484.878965
+2.329435, 0.000000, 1484.878965
+2.339329, 0.000000, 1484.878965
+2.349389, 0.000000, 1484.878965
+2.359454, 0.000000, 1484.878965
+2.369832, 0.000000, 1484.878965
+2.379390, 0.000000, 1484.878965
+2.389381, 0.000000, 1484.878965
+2.399429, 0.000000, 1484.878965
+2.409394, 0.000000, 1484.878965
+2.419367, 0.000000, 1484.878965
+2.429384, 0.000000, 1484.878965
+2.439408, 0.000000, 1484.878965
+2.449391, 0.000000, 1484.878965
+2.459343, 0.000000, 1484.878965
+2.469424, 0.000000, 1484.878965
+2.479357, 0.000000, 1484.878965
+2.489388, 0.000000, 1484.878965
+2.499413, 0.000000, 1484.878965
+2.510081, 0.000000, 1484.878965
+2.519397, 0.000000, 1484.878965
+2.529342, 0.000000, 1484.878965
+2.539372, 0.000000, 1484.878965
+2.549674, 0.000000, 1484.878965
+2.559586, 0.000000, 1484.878965
+2.569807, 0.000000, 1484.878965
+2.579362, 0.000000, 1484.878965
+2.589325, 0.000000, 1484.878965
+2.599300, 0.000000, 1484.878965
+2.609436, 0.000000, 1484.878965
+2.619476, 0.000000, 1484.878965
+2.629668, 0.000000, 1484.878965
+2.639301, 0.000000, 1484.878965
+2.649411, 0.000000, 1484.878965
+2.659301, 0.000000, 1484.878965
+2.669336, 0.000000, 1484.878965
+2.679460, 0.000000, 1484.878965
+2.689691, 0.000000, 1484.878965
+2.699310, 0.000000, 1484.878965
+2.710046, 0.000000, 1484.878965
+2.719584, 0.000000, 1484.878965
+2.729333, 0.000000, 1484.878965
+2.739288, 0.000000, 1484.878965
+2.749320, 0.000000, 1484.878965
+2.759517, 0.000000, 1484.878965
+2.769811, 0.000000, 1484.878965
+2.779463, 0.000000, 1484.878965
+2.789708, 0.000000, 1484.878965
+2.799310, 12.000000, 1484.878965
+2.809361, 12.000000, 1484.878965
+2.819345, 12.000000, 1484.943934
+2.829470, 12.000000, 1485.117183
+2.839666, 12.000000, 1485.355402
+2.849432, 12.000000, 1485.680245
+2.859547, 12.000000, 1486.091712
+2.869422, 12.000000, 1486.589805
+2.879352, 12.000000, 1487.152866
+2.889431, 12.000000, 1487.802552
+2.899538, 12.000000, 1488.538863
+2.909416, 12.000000, 1489.340142
+2.919676, 12.000000, 1490.163078
+2.929470, 11.856977, 1491.072638
+2.939341, 10.941776, 1492.090480
+2.949422, 9.709468, 1493.086665
+2.959343, 9.484298, 1494.212787
+2.969480, 9.024482, 1495.360566
+2.979482, 8.408468, 1496.616625
+2.989402, 7.584528, 1497.851029
+2.999839, 7.318006, 1499.193713
+3.009487, 6.726255, 1500.601366
+3.019345, 5.691274, 1502.030675
+3.029433, 5.445505, 1503.503297
+3.039661, 5.201068, 1505.019231
+3.049419, 4.805405, 1506.556821
+3.059323, 4.164102, 1508.116067
+3.069465, 3.901448, 1509.696970
+3.079658, 3.715078, 1511.321185
+3.089408, 3.656437, 1512.902087
+3.099346, 3.325052, 1514.504646
+3.109498, 3.310343, 1516.128861
+3.119381, 3.348580, 1517.753076
+3.129434, 3.031678, 1519.377291
+3.139461, 3.165136, 1520.979850
+3.149456, 3.276186, 1522.604064
+3.159650, 3.130954, 1524.228279
+3.169436, 3.017931, 1525.852494
+3.179542, 2.968543, 1527.455053
+3.189425, 3.107515, 1529.079268
+3.199654, 3.010200, 1530.681827
+3.209442, 3.078322, 1532.306042
+3.219518, 2.953745, 1533.908601
+3.229434, 3.021034, 1535.511159
+3.239303, 4.196084, 1537.113718
+3.249474, 2.523334, 1538.716277
+3.259481, 2.549791, 1540.318836
+3.269395, 2.856174, 1541.943051
+3.279668, 2.830169, 1543.545609
+3.289491, 2.903769, 1545.148168
+3.299343, 2.930722, 1546.729071
+3.309430, 2.915555, 1548.331629
+3.319847, 2.887528, 1549.934188
+3.329444, 3.022588, 1551.515091
+3.339468, 2.922583, 1553.095993
+3.349700, 3.155171, 1554.676896
+3.359307, 2.959473, 1556.257798
+3.369439, 2.963462, 1557.817045
+3.379865, 3.151337, 1559.397947
+3.389482, 3.237455, 1560.957194
+3.399670, 3.075969, 1562.538096
+3.409431, 3.127616, 1564.097342
+3.419355, 3.012946, 1565.656589
+3.429472, 3.094794, 1567.237491
+3.439645, 2.986884, 1568.796738
+3.449433, 3.228489, 1570.355984
+3.459540, 3.042079, 1571.915230
+3.469425, 3.052375, 1573.474477
+3.479338, 3.083112, 1575.055379
+3.489491, 2.926137, 1576.614626
+3.499576, 2.990285, 1578.152216
+3.509412, 3.204911, 1579.711462
+3.519368, 3.134930, 1581.270709
+3.529436, 3.050871, 1582.829955
+3.539512, 3.012085, 1584.389201
+3.549482, 3.160836, 1585.948448
+3.559848, 3.076263, 1587.486038
+3.569480, 2.996910, 1589.045284
+3.579466, 2.963210, 1590.604530
+3.589392, 3.113684, 1592.142121
+3.599645, 3.029053, 1593.679711
+3.609466, 3.111230, 1595.238957
+3.619478, 3.001191, 1596.776547
+3.629414, 3.083010, 1598.335794
+3.639639, 2.977469, 1599.873384
+3.649431, 3.061646, 1601.410974
+3.659343, 2.956478, 1602.970220
+3.669437, 3.040410, 1604.507810
+3.679660, 3.096927, 1606.067057
+3.689414, 3.104633, 1607.582991
+3.699466, 3.092523, 1609.120581
+3.709581, 3.079314, 1610.658171
+3.719844, 3.069195, 1612.195761
+3.729455, 3.060854, 1613.755008
+3.739454, 2.890971, 1615.270942
+3.749456, 3.120861, 1616.808532
+3.759652, 2.943141, 1618.367778
+3.769565, 2.963262, 1619.905368
+3.779406, 3.162518, 1621.442958
+3.789419, 3.096059, 1622.958892
+3.799647, 2.857986, 1624.496482
+3.809451, 3.063847, 1626.034073
+3.819535, 3.048330, 1627.550007
+3.829423, 3.158748, 1629.087597
+3.839290, 3.053598, 1630.625187
+3.849534, 2.974064, 1632.162777
+3.859585, 2.947301, 1633.678711
+3.869342, 3.104581, 1635.194645
+3.879651, 3.025376, 1636.753891
+3.889461, 3.112302, 1638.269825
+3.899471, 3.006920, 1639.785759
+3.909503, 3.093474, 1641.323349
+3.919650, 2.992726, 1642.860940
+3.929493, 3.081710, 1644.376873
+3.939457, 2.981346, 1645.914464
+3.949675, 2.908199, 1647.430398
+3.959566, 3.045687, 1648.946332
+3.969420, 3.126825, 1650.505578
+3.979850, 2.816686, 1652.021512
+3.989429, 3.120514, 1653.537446
+3.999650, 3.009544, 1655.053380
+4.009434, 3.053283, 1656.590970
+4.019490, 2.769875, 1658.106904
+4.029473, 2.932334, 1659.644494
+4.039639, 2.903060, 1661.182084
+4.049524, 3.176347, 1662.698018
+4.059463, 2.998063, 1664.213952
+4.069518, 3.013137, 1665.729886
+4.079855, 3.051287, 1667.267476
+4.089451, 3.065561, 1668.783410
+4.099514, 2.901543, 1670.299344
+4.109450, 2.971613, 1671.793622
+4.119835, 3.035151, 1673.331212
+4.129459, 3.052225, 1674.847146
+4.139393, 2.884954, 1676.363080
+4.149517, 3.114730, 1677.879014
+4.159643, 3.101587, 1679.373292
+4.169483, 3.212047, 1680.889226
+4.179478, 2.947840, 1682.405160
+4.189417, 3.111024, 1683.921094
+4.199352, 3.081820, 1685.415371
+4.209534, 3.196368, 1686.931305
+4.219472, 2.937203, 1688.447239
+4.229358, 3.102203, 1689.963173
+4.239581, 4.044265, 1691.479107
+4.249471, 2.568287, 1692.995041
+4.259564, 2.592233, 1694.510975
+4.269489, 2.874029, 1696.048565
+4.279649, 2.848913, 1697.564499
+4.289504, 3.102328, 1699.058777
+4.299467, 2.913154, 1700.574711
+4.309396, 2.926342, 1702.090645
+4.319635, 3.127416, 1703.584923
+4.329463, 3.066398, 1705.122513
+4.339462, 3.157757, 1706.616790
+4.349523, 3.054520, 1708.132724
+4.359685, 2.982710, 1709.627002
+4.369520, 3.124646, 1711.142936
+4.379481, 2.887285, 1712.658870
+4.389476, 3.058346, 1714.174804
+4.399662, 3.028207, 1715.647426
+4.409523, 3.140828, 1717.163360
+4.419532, 3.042732, 1718.679293
+4.429478, 3.131501, 1720.195227
+4.439668, 3.033777, 1721.667849
+4.449451, 3.127131, 1723.183783
+4.459519, 3.031674, 1724.699717
+4.469529, 3.125338, 1726.193995
+4.479652, 3.029621, 1727.709929
+4.489541, 3.123096, 1729.204206
+4.499285, 2.865452, 1730.720140
+4.509567, 3.035110, 1732.236074
+4.519358, 3.169872, 1733.752008
+4.529256, 3.046239, 1735.246286
+4.539423, 2.956041, 1736.762220
+4.549418, 3.092368, 1738.278154
+4.559287, 3.017417, 1739.772432
+4.569392, 3.113609, 1741.266709
+4.579389, 3.015401, 1742.804300
+4.589391, 3.107141, 1744.298577
+4.599271, 3.010950, 1745.792855
+4.609237, 3.104538, 1747.308789
+4.619397, 2.847040, 1748.803067
+4.629258, 3.016751, 1750.340657
+4.639391, 3.151513, 1751.834935
+4.649382, 3.027871, 1753.329212
+4.659249, 2.937667, 1754.845146
+4.669717, 3.073994, 1756.361080
+4.679709, 2.837164, 1757.877014
+4.689305, 3.009530, 1759.371292
+4.699414, 2.979957, 1760.865570
+4.709265, 3.254504, 1762.381504
+4.719267, 2.918277, 1763.875782
+4.729387, 3.014535, 1765.391715
+4.739263, 2.964507, 1766.885993
+4.749394, 3.242657, 1768.401927
+4.759241, 3.073913, 1769.917861
+4.769396, 2.934311, 1771.412139
+4.779436, 2.891905, 1772.928073
+4.789376, 3.055522, 1774.444007
+4.799259, 2.823787, 1775.938285
+4.809430, 2.993720, 1777.454219
+4.819715, 2.961969, 1778.948496
+4.829635, 3.235854, 1780.442774
+4.839637, 3.061570, 1781.958708
+4.849506, 3.081810, 1783.474642
+4.859505, 2.801335, 1784.968920
+4.869352, 3.134735, 1786.484854
+4.879650, 3.036602, 1787.979131
+4.889446, 3.084670, 1789.495065
+4.899459, 2.966217, 1790.989343
+4.909516, 3.056512, 1792.483621
+4.919642, 2.963315, 1793.999555
+4.929515, 3.058695, 1795.493833
+4.939540, 2.963436, 1796.988110
+4.949486, 3.056827, 1798.504044
+4.959635, 2.960935, 1800.019978
+4.969264, 3.054371, 1801.514256
+4.979559, 3.120496, 1803.030190
+4.989369, 2.975943, 1804.502812
+4.999639, 3.049612, 1806.018745
+5.009522, 3.114635, 1807.513023
+5.019539, 2.811284, 1809.028957
+5.029502, 3.124778, 1810.523235
+5.039639, 3.020720, 1812.017513
+5.049531, 3.069436, 1813.511790
+5.059571, 2.952407, 1815.006068
+5.069509, 3.205185, 1816.522002
+5.079646, 3.035861, 1818.016280
+5.089525, 3.224397, 1819.532214
+5.099572, 2.870103, 1821.026492
+5.109284, 3.134886, 1822.542426
+5.119546, 3.015399, 1824.036703
+5.129816, 3.065232, 1825.530981
+5.139566, 2.951614, 1827.025259
+5.149507, 3.044046, 1828.541193
+5.159648, 2.951048, 1830.035471
+5.169497, 3.208013, 1831.551405
+5.179545, 2.874516, 1833.045682
+5.189501, 3.137284, 1834.539960
+5.199633, 3.012938, 1836.034238
+5.209521, 3.222545, 1837.528516
+5.219560, 3.032578, 1839.044450
+5.229503, 3.056672, 1840.538727
+5.239594, 4.237957, 1842.033005
+5.249532, 2.592533, 1843.527283
+5.259542, 2.486074, 1845.043217
+5.269566, 2.894539, 1846.515838
+5.279650, 3.123100, 1848.010116
+5.289252, 3.165125, 1849.504394
+5.299546, 3.307151, 1850.998672
+5.309525, 3.213976, 1852.471293
+5.319632, 3.141194, 1853.943915
+5.329458, 3.284019, 1855.459849
+5.339575, 3.050528, 1856.932470
+5.349266, 3.226635, 1858.448404
+5.359682, 3.201591, 1859.921026
+5.369278, 3.319115, 1861.436960
+5.379549, 3.063938, 1862.931237
+5.389254, 3.071781, 1864.447171
+5.399651, 2.961775, 1865.919793
+5.409641, 3.063785, 1867.435727
+5.419564, 0.000000, 1868.930005
+5.429518, 0.000000, 1870.424282
+5.439596, 0.000000, 1871.918560
+5.449485, 0.000000, 1873.347869
+5.459469, 0.000000, 1874.733866
+5.469640, 0.000000, 1876.098207
+5.479575, 0.000000, 1877.419235
+5.489492, 0.000000, 1878.675294
+5.499454, 0.000000, 1879.909698
+5.509441, 0.000000, 1881.100789
+5.519669, 0.000000, 1882.270223
+5.529268, 0.000000, 1883.396346
+5.539531, 0.000000, 1884.479156
+5.549491, 0.000000, 1885.518653
+5.559591, 0.000000, 1886.536495
+5.569479, 0.000000, 1887.532680
+5.579553, 0.000000, 1888.485553
+5.589263, 0.000000, 1889.416769
+5.599628, 0.000000, 1890.283017
+5.609531, 0.000000, 1891.149265
+5.619452, 0.000000, 1891.993857
+5.629254, 0.000000, 1892.795136
+5.639595, 0.000000, 1893.574759
+5.649382, 0.000000, 1894.332726
+5.659518, 0.000000, 1895.047381
+5.669510, 0.000000, 1895.740379
+5.679630, 0.000000, 1896.433378
+5.689513, 0.000000, 1897.083064
+5.699559, 0.000000, 1897.689437
+5.709897, 0.000000, 1898.295811
+5.719663, 0.000000, 1898.880528
+5.729785, 0.000000, 1899.443590
+5.739550, 0.000000, 1899.984995
+5.749477, 0.000000, 1900.504743
+5.759631, 0.000000, 1901.002836
+5.769499, 0.000000, 1901.500928
+5.779545, 0.000000, 1901.955709
+5.789477, 0.000000, 1902.388833
+5.799588, 0.000000, 1902.821957
+5.809530, 0.000000, 1903.233424
+5.819550, 0.000000, 1903.623236
+5.829288, 0.000000, 1903.991391
+5.839583, 0.000000, 1904.359547
+5.849261, 0.000000, 1904.706046
+5.859488, 0.000000, 1905.030889
+5.869375, 0.000000, 1905.334076
+5.879665, 0.000000, 1905.658919
+5.889507, 0.000000, 1905.918793
+5.899530, 0.000000, 1906.200324
+5.909520, 0.000000, 1906.460198
+5.919633, 0.000000, 1906.720073
+5.929467, 0.000000, 1906.958291
+5.939535, 0.000000, 1907.174853
+5.949423, 0.000000, 1907.391415
+5.959583, 0.000000, 1907.586320
+5.969500, 0.000000, 1907.781226
+5.979548, 0.000000, 1907.954476
+5.989236, 0.000000, 1908.106069
+5.999631, 0.000000, 1908.279319
+6.009506, 0.000000, 1908.409256
+6.019548, 0.000000, 1908.560849
+6.029525, 0.000000, 1908.669130
+6.039582, 0.000000, 1908.799068
+6.049527, 0.000000, 1908.907349
+6.059538, 0.000000, 1909.015630
+6.069537, 0.000000, 1909.102254
+6.079650, 0.000000, 1909.188879
+6.089474, 0.000000, 1909.253848
+6.099533, 0.000000, 1909.318816
+6.109497, 0.000000, 1909.383785
+6.119587, 0.000000, 1909.448754
+6.129255, 0.000000, 1909.492066
+6.139524, 0.000000, 1909.513722
+6.149479, 0.000000, 1909.557035
+6.159584, 0.000000, 1909.578691
+6.169260, 0.000000, 1909.600347
+6.179536, 0.000000, 1909.600347
+6.189396, 0.000000, 1909.600347
+6.199574, 0.000000, 1909.600347
+6.209506, 0.000000, 1909.600347
+6.219521, 0.000000, 1909.600347
+6.229482, 0.000000, 1909.600347
+6.239565, 0.000000, 1909.600347
+6.249254, 0.000000, 1909.600347
+6.259521, 0.000000, 1909.600347
+6.269506, 0.000000, 1909.600347
+6.279589, 0.000000, 1909.600347
+6.289535, 0.000000, 1909.600347
+6.299542, 0.000000, 1909.600347
+6.309460, 0.000000, 1909.600347
+6.319858, 0.000000, 1909.600347
+6.329537, 0.000000, 1909.600347
+6.339535, 0.000000, 1909.600347
+6.349231, 0.000000, 1909.600347
+6.359649, 0.000000, 1909.600347
+6.369534, 0.000000, 1909.600347
+6.379535, 0.000000, 1909.600347
diff --git a/bot3/control_loops/shooter/shooter_lib_test.cc b/bot3/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..062e507
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,174 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/shooter/shooter.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace bot3 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterMotorSimulation {
+ public:
+ // Constructs a shooter simulation. I'm not sure how the construction of
+ // the queue (my_shooter_loop_) actually works (same format as wrist).
+ ShooterMotorSimulation()
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
+ my_shooter_loop_(".bot3.control_loops.shooter",
+ 0x78d8e372, ".bot3.control_loops.shooter.goal",
+ ".bot3.control_loops.shooter.position",
+ ".bot3.control_loops.shooter.output",
+ ".bot3.control_loops.shooter.status") {
+ }
+
+ // Sends a queue message with the position of the shooter.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+ my_shooter_loop_.position.MakeMessage();
+ position->position = shooter_plant_->Y(0, 0);
+ position.Send();
+ }
+
+ // Simulates shooter for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ shooter_plant_->U << my_shooter_loop_.output->voltage;
+ shooter_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+ private:
+ ShooterLoop my_shooter_loop_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ ShooterTest() : my_shooter_loop_(".bot3.control_loops.shooter",
+ 0x78d8e372,
+ ".bot3.control_loops.shooter.goal",
+ ".bot3.control_loops.shooter.position",
+ ".bot3.control_loops.shooter.output",
+ ".bot3.control_loops.shooter.status"),
+ shooter_motor_(&my_shooter_loop_),
+ shooter_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+
+ // Update the robot state. Without this, the Iteration of the control loop
+ // will stop all the motors and the shooter won't go anywhere.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_NEAR(my_shooter_loop_.goal->velocity,
+ my_shooter_loop_.status->average_velocity,
+ 10.0);
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to
+ // shared memory that is no longer valid.
+ ShooterLoop my_shooter_loop_;
+
+ // Create a control loop and simulation.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ SendDSPacket(true);
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ VerifyNearGoal();
+ my_shooter_loop_.output.FetchLatest();
+ EXPECT_EQ(my_shooter_loop_.output->voltage, 0.0);
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(450.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+ VerifyNearGoal();
+
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+ }
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+TEST_F(ShooterTest, Disabled) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(i > 50);
+ }
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter_main.cc b/bot3/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..9907cfe
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "bot3/control_loops/shooter/shooter.h"
+
+#include "aos/atom_code/init.h"
+
+int main() {
+ ::aos::Init();
+ bot3::control_loops::ShooterMotor shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/control_loops/shooter/shooter_motor.q b/bot3/control_loops/shooter/shooter_motor.q
new file mode 100644
index 0000000..87eaa16
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor.q
@@ -0,0 +1,31 @@
+package bot3.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group ShooterLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // Goal velocity in rad/sec
+ double velocity;
+ };
+
+ message Status {
+ // True if the shooter is up to speed.
+ bool ready;
+ // The average velocity over the last 0.1 seconds.
+ double average_velocity;
+ };
+
+ message Position {
+ // The angle of the shooter wheel measured in rad/sec.
+ double position;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue Status status;
+};
+
+queue_group ShooterLoop shooter;
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
new file mode 100644
index 0000000..0617554
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -0,0 +1,47 @@
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+
+#include <vector>
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00992127884628, 0.0, 0.984297191531;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.00398899915072, 0.795700859132;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackController<2, 1, 1> MakeShooterController() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.08429719153, 29.2677479765;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 0.955132813139, 0.50205697652;
+ return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
+}
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeShooterPlantCoefficients());
+ return StateFeedbackPlant<2, 1, 1>(plants);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
+ ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<2, 1, 1>(MakeShooterController());
+ return StateFeedbackLoop<2, 1, 1>(controllers);
+}
+
+} // namespace control_loops
+} // namespace bot3
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
new file mode 100644
index 0000000..1721d30
--- /dev/null
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -0,0 +1,20 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace bot3 {
+namespace control_loops {
+
+StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();
+
+StateFeedbackController<2, 1, 1> MakeShooterController();
+
+StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
+
+} // namespace control_loops
+} // namespace bot3
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index d647d39..5b31505 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -3,6 +3,7 @@
#include "aos/common/util/wrapping_counter.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/input/usb_receiver.h"
@@ -11,6 +12,7 @@
#endif
using ::bot3::control_loops::drivetrain;
+using ::bot3::control_loops::shooter;
using ::frc971::sensors::gyro;
using ::aos::util::WrappingCounter;
using ::frc971::USBReceiver;
@@ -22,7 +24,7 @@
// encoder or not.
inline double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (44.0 / 32.0 /*encoder gears*/) * // the encoders are on the wheels.
+ (32.0 / 44.0 /*encoder gears*/) * // the encoders are on the wheels.
(3.5 /*wheel diameter*/ * 2.54 / 100 * M_PI);
}
@@ -54,8 +56,11 @@
.right_encoder(drivetrain_translate(data()->main.wrist))
.left_encoder(drivetrain_translate(data()->main.shooter))
.Send();
- LOG(DEBUG, "right: %d left: %d angle: %lld \n",
- data()->main.wrist, data()->main.shooter, data()->gyro_angle);
+ LOG(DEBUG, "right: %lf left: %lf angle: %lld \n",
+ drivetrain_translate(data()->main.wrist),
+ drivetrain_translate(data()->main.shooter), data()->gyro_angle);
+
+ shooter.position.MakeWithBuilder().position(drivetrain_translate(data()->main.shooter)).Send();
}
WrappingCounter top_rise_;
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
index b428dc2..3e1cfa7 100644
--- a/bot3/input/input.gyp
+++ b/bot3/input/input.gyp
@@ -24,6 +24,7 @@
],
'dependencies': [
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
diff --git a/bot3/output/atom_motor_writer.cc b/bot3/output/atom_motor_writer.cc
index 95dec0d..43a45a3 100644
--- a/bot3/output/atom_motor_writer.cc
+++ b/bot3/output/atom_motor_writer.cc
@@ -8,8 +8,10 @@
#include "frc971/queues/Piston.q.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
using ::bot3::control_loops::drivetrain;
+using ::bot3::control_loops::shooter;
using ::frc971::control_loops::shifters;
namespace bot3 {
@@ -43,12 +45,13 @@
SetSolenoid(8, !shifters->set);
}
- /*if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- SetPWMOutput(4, shooter.output->voltage / 12.0, kVictorBounds);
+ shooter.output.FetchLatest();
+ if (shooter.output.IsNewerThanMS(kOutputMaxAgeMS)) {
+ SetPWMOutput(2, shooter.output->voltage / 12.0, kVictorBounds);
} else {
- DisablePWMOutput(4);
+ DisablePWMOutput(2);
LOG(WARNING, "shooter not new enough\n");
- }*/
+ }
// TODO(danielp): Add stuff for intake and shooter.
}
};
diff --git a/bot3/output/output.gyp b/bot3/output/output.gyp
index 219f501..c207cfb 100644
--- a/bot3/output/output.gyp
+++ b/bot3/output/output.gyp
@@ -34,6 +34,7 @@
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
],