Import shooter from 2013
Taken from commit a085044
I liberally delete some things in the Python code to get
things compiling. I also added TODOs where I am unsure whether
my changes are appropriate.
Change-Id: I4f3ae9b69ca9c2b1917e73136c766275a44d3665
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
new file mode 100644
index 0000000..f34a1f1
--- /dev/null
+++ b/y2016/control_loops/python/shooter.py
@@ -0,0 +1,72 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Shooter(control_loop.ControlLoop):
+ def __init__(self):
+ super(Shooter, self).__init__("Shooter")
+ # Stall Torque in N m
+ self.stall_torque = 0.49819248
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0 - 1500.0
+ # Free Current in Amps
+ self.free_current = 1.4
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.0032
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current / 2
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 11.0 / 34.0
+ # Control loop time step
+ self.dt = 0.005
+
+ # 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.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ 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):
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['y2016', 'control_loops', 'shooter']
+ shooter = Shooter()
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter],
+ namespaces=namespaces)
+ 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))