Made shooter python file velocity-only.
Still need to fix a couple of thins in the C++ files.
diff --git a/bot3/control_loops/python/control_loop.py b/bot3/control_loops/python/control_loop.py
deleted file mode 100644
index 5ff724b..0000000
--- a/bot3/control_loops/python/control_loop.py
+++ /dev/null
@@ -1,295 +0,0 @@
-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 \"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
deleted file mode 100644
index a40bfe2..0000000
--- a/bot3/control_loops/python/controls.py
+++ /dev/null
@@ -1,101 +0,0 @@
-#!/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
deleted file mode 100755
index 0e791cb..0000000
--- a/bot3/control_loops/python/drivetrain.py
+++ /dev/null
@@ -1,162 +0,0 @@
-#!/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
deleted file mode 100644
index a217728..0000000
--- a/bot3/control_loops/python/libcdd.py
+++ /dev/null
@@ -1,129 +0,0 @@
-#!/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
deleted file mode 100755
index 1d68f51..0000000
--- a/bot3/control_loops/python/shooter.py
+++ /dev/null
@@ -1,136 +0,0 @@
-#!/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([.881])
- print self.K
- self.R_LQR = numpy.matrix([[0]])
- 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 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/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
index ab5dbc7..e8113a4 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.cc
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -1,10 +1,10 @@
-#include "bot3/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/control_loops/shooter/shooter_motor_plant.h"
#include <vector>
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace frc971 {
namespace control_loops {
StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients() {
@@ -13,7 +13,7 @@
Eigen::Matrix<double, 1, 1> B;
B << 0.533205953514;
Eigen::Matrix<double, 1, 1> C;
- C << 0;
+ C << 1;
Eigen::Matrix<double, 1, 1> D;
D << 0;
Eigen::Matrix<double, 1, 1> U_max;
@@ -25,9 +25,9 @@
StateFeedbackController<1, 1, 1> MakeShooterController() {
Eigen::Matrix<double, 1, 1> L;
- L << 29.7111780621;
+ L << 0.539057756738;
Eigen::Matrix<double, 1, 1> K;
- K << 0.758151303088;
+ K << 0.354567977894;
return StateFeedbackController<1, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
@@ -44,4 +44,4 @@
}
} // namespace control_loops
-} // namespace bot3
+} // namespace frc971
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
index 1ab9702..18ca817 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.h
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
-#define BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#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 frc971 {
namespace control_loops {
StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients();
@@ -15,6 +15,6 @@
StateFeedbackLoop<1, 1, 1> MakeShooterLoop();
} // namespace control_loops
-} // namespace bot3
+} // namespace frc971
-#endif // BOT3_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_SHOOTER_MOTOR_PLANT_H_
diff --git a/bot3/control_loops/update_shooter.sh b/bot3/control_loops/update_shooter.sh
index db98547..26e7ae3 100755
--- a/bot3/control_loops/update_shooter.sh
+++ b/bot3/control_loops/update_shooter.sh
@@ -2,4 +2,4 @@
#
# Updates the shooter controller.
-./python/shooter.py shooter/shooter_motor_plant.h shooter/shooter_motor_plant.cc
+../../frc971/control_loops/python/shooter.py shooter/shooter_motor_plant.h shooter/shooter_motor_plant.cc