commit 9ffe298a2ba4e998d78dbb541915b7a702cfc3a9
author Philipp Schrader <philipp@peloton-tech.com> Wed Dec 07 20:51:08 2016 -0800
committer Philipp Schrader <philipp.schrader@gmail.com> Thu Dec 15 04:11:41 2016 +0000
tree 2bbc840ff14de8bef1b29008198235a8f5d5a6e1
parent 70810b9b466595f25c48ab2c424d5be7b0a92264

Break out the CIM class to avoid duplication

Before this patch we had 7 identical copies of the CIM class. Now it's
in its own module.

More de-duplication to come.

Change-Id: I598224020e50e8ec63584333dc080d34a2407cde

y2014_bot3/control_loops/python/drivetrain.py

diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index 4c3577b..0dc2466 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -14,48 +14,6 @@
 
 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 
-class CIM(control_loop.ControlLoop):
-  def __init__(self):
-    super(CIM, self).__init__("CIM")
-    # Stall Torque in N m
-    self.stall_torque = 2.42
-    # 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 CIM in kg m^2
-    self.J = 0.0001
-    # Resistance of the motor, divided by 2 to account for the 2 motors
-    self.resistance = 12.0 / self.stall_current
-    # Motor velocity constant
-    self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
-              (12.0 - self.resistance * self.free_current))
-    # Torque constant
-    self.Kt = self.stall_torque / self.stall_current
-    # Control loop time step
-    self.dt = 0.005
-
-    # State feedback matrices
-    self.A_continuous = numpy.matrix(
-        [[-self.Kt / self.Kv / (self.J * self.resistance)]])
-    self.B_continuous = numpy.matrix(
-        [[self.Kt / (self.J * self.resistance)]])
-    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.PlaceControllerPoles([0.01])
-    self.PlaceObserverPoles([0.01])
-
-    self.U_max = numpy.matrix([[12.0]])
-    self.U_min = numpy.matrix([[-12.0]])
-
-    self.InitializeState()
-
 
 class Drivetrain(control_loop.ControlLoop):
   def __init__(self, name="Drivetrain", left_low=True, right_low=True):

y2014_bot3/control_loops/python/polydrivetrain.py

diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index e047c11..1d426d9 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -6,6 +6,7 @@
 from y2014_bot3.control_loops.python import drivetrain
 from frc971.control_loops.python import control_loop
 from frc971.control_loops.python import controls
+from frc971.control_loops.python.cim import CIM
 from matplotlib import pylab
 
 import gflags
@@ -196,8 +197,8 @@
     self.right_gear = VelocityDrivetrain.LOW
     self.left_shifter_position = 0.0
     self.right_shifter_position = 0.0
-    self.left_cim = drivetrain.CIM()
-    self.right_cim = drivetrain.CIM()
+    self.left_cim = CIM()
+    self.right_cim = CIM()
 
   def IsInGear(self, gear):
     return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
@@ -428,8 +429,7 @@
 
       dog_loop_writer.Write(argv[1], argv[2])
 
-      cim_writer = control_loop.ControlLoopWriter(
-          "CIM", [drivetrain.CIM()])
+      cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
 
       cim_writer.Write(argv[3], argv[4])
       return