diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index ed180d8..fe20228 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -5,6 +5,49 @@
 import sys
 from matplotlib import pylab
 
+
+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.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
+    # Control loop time step
+    self.dt = 0.01
+
+    # State feedback matrices
+    self.A_continuous = numpy.matrix(
+        [[-self.Kt / self.Kv / (self.J * self.R)]])
+    self.B_continuous = numpy.matrix(
+        [[self.Kt / (self.J * 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.PlaceControllerPoles([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, left_low=True, right_low=True):
     super(Drivetrain, self).__init__("Drivetrain")
@@ -26,7 +69,7 @@
     # 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
+    self.R = (12.0 / self.stall_current / 4 + 0.03) / (0.93 ** 2.0)
     # Motor velocity constant
     self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
                (12.0 - self.R * self.free_current))
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 97ab3be..1c7aca2 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -179,6 +179,7 @@
                   self.CurrentDrivetrain().r)
     low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
                  self.CurrentDrivetrain().r)
+    print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
     high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
                    self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
     low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
@@ -195,6 +196,13 @@
         else:
           print gear_name, "Shifting to low"
 
+    # Shift algorithm improvements.
+    # TODO(aschuh):
+    # It takes time to shift.  Shifting down for 1 cycle doesn't make sense
+    # because you will end up slower than without shifting.  Figure out how
+    # to include that info.
+    # If the driver is still in high gear, but isn't asking for the extra power
+    # from low gear, don't shift until he asks for it.
     return high_power > low_power
 
   def FilterVelocity(self, throttle):
@@ -293,7 +301,7 @@
 
 
 def main(argv):
-  drivetrain = VelocityDrivetrain()
+  vdrivetrain = VelocityDrivetrain()
 
   vl_plot = []
   vr_plot = []
@@ -303,47 +311,66 @@
   t_plot = []
   left_gear_plot = []
   right_gear_plot = []
-  drivetrain.left_high = True
-  drivetrain.right_high = True
+  vdrivetrain.left_high = True
+  vdrivetrain.right_high = True
 
-  if drivetrain.left_high:
+  print "K is", vdrivetrain.CurrentDrivetrain().K
+
+  if vdrivetrain.left_high:
     print "Left is high"
   else:
     print "Left is low"
-  if drivetrain.right_high:
+  if vdrivetrain.right_high:
     print "Right is high"
   else:
     print "Right is low"
 
-  for t in numpy.arange(0, 2.0, drivetrain.dt):
+  for t in numpy.arange(0, 2.0, vdrivetrain.dt):
     if t < 1.0:
-      drivetrain.Update(throttle=0.60, steering=0.3)
+      vdrivetrain.Update(throttle=0.60, steering=0.3)
     elif t < 1.5:
-      drivetrain.Update(throttle=0.60, steering=-0.3)
+      vdrivetrain.Update(throttle=0.60, steering=-0.3)
     else:
-      drivetrain.Update(throttle=0.0, steering=0.3)
+      vdrivetrain.Update(throttle=0.0, steering=0.3)
     t_plot.append(t)
-    vl_plot.append(drivetrain.X[0, 0])
-    vr_plot.append(drivetrain.X[1, 0])
-    ul_plot.append(drivetrain.U[0, 0])
-    ur_plot.append(drivetrain.U[1, 0])
-    left_gear_plot.append(drivetrain.left_high * 2.0 - 10.0)
-    right_gear_plot.append(drivetrain.right_high * 2.0 - 10.0)
+    vl_plot.append(vdrivetrain.X[0, 0])
+    vr_plot.append(vdrivetrain.X[1, 0])
+    ul_plot.append(vdrivetrain.U[0, 0])
+    ur_plot.append(vdrivetrain.U[1, 0])
+    left_gear_plot.append(vdrivetrain.left_high * 2.0 - 10.0)
+    right_gear_plot.append(vdrivetrain.right_high * 2.0 - 10.0)
 
-    fwd_velocity = (drivetrain.X[1, 0] + drivetrain.X[0, 0]) / 2
-    turn_velocity = (drivetrain.X[1, 0] - drivetrain.X[0, 0])
+    fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+    turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
     if fwd_velocity < 0.0000001:
       radius_plot.append(turn_velocity)
     else:
       radius_plot.append(turn_velocity / fwd_velocity)
 
+  cim_velocity_plot = []
+  cim_voltage_plot = []
+  cim_time = []
+  cim = drivetrain.CIM()
+  R = numpy.matrix([[300]])
+  for t in numpy.arange(0, 0.5, cim.dt):
+    U = numpy.clip(cim.K * (R - cim.X) + R / cim.Kv, cim.U_min, cim.U_max)
+    cim.Update(U)
+    cim_velocity_plot.append(cim.X[0, 0])
+    cim_voltage_plot.append(U[0, 0] * 10)
+    cim_time.append(t)
+  pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
+  pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+  pylab.legend()
+  pylab.show()
+
+
   pylab.plot(t_plot, vl_plot, label='left velocity')
   pylab.plot(t_plot, vr_plot, label='right velocity')
-  pylab.plot(t_plot, ul_plot, label='left power')
-  pylab.plot(t_plot, ur_plot, label='right power')
+  pylab.plot(t_plot, ul_plot, label='left voltage')
+  pylab.plot(t_plot, ur_plot, label='right voltage')
   pylab.plot(t_plot, radius_plot, label='radius')
-  pylab.plot(t_plot, left_gear_plot, label='left_gear')
-  pylab.plot(t_plot, right_gear_plot, label='right_gear')
+  pylab.plot(t_plot, left_gear_plot, label='left gear high')
+  pylab.plot(t_plot, right_gear_plot, label='right gear high')
   pylab.legend()
   pylab.show()
   return 0