diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index fe20228..27bebbc 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -41,6 +41,7 @@
                                                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]])
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 0251408..da9d414 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -117,7 +117,7 @@
     # FF * X = U (steady state)
     self.FF = self.B.I * (numpy.eye(2) - self.A)
 
-    self.PlaceControllerPoles([0.5, 0.5])
+    self.PlaceControllerPoles([0.6, 0.6])
     self.PlaceObserverPoles([0.02, 0.02])
 
     self.G_high = self._drivetrain.G_high
@@ -132,8 +132,14 @@
 
 
 class VelocityDrivetrain(object):
+  HIGH = 'high'
+  LOW = 'low'
+  SHIFTING_UP = 'up'
+  SHIFTING_DOWN = 'down'
+
   def __init__(self):
-    self.drivetrain_low_low = VelocityDrivetrainModel(left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
+    self.drivetrain_low_low = VelocityDrivetrainModel(
+        left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
     self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
     self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
     self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
@@ -171,42 +177,65 @@
     # inertia.  A value of 1 is no throttle negative inertia.
     self.ttrust = 1.0
 
-    self.left_high = False
-    self.right_high = False
+    self.left_gear = VelocityDrivetrain.LOW
+    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()
+
+  def IsInGear(self, gear):
+    return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+
+  def MotorRPM(self, shifter_position, velocity):
+    if shifter_position > 0.5:
+      return (velocity / self.CurrentDrivetrain().G_high /
+              self.CurrentDrivetrain().r)
+    else:
+      return (velocity / self.CurrentDrivetrain().G_low /
+              self.CurrentDrivetrain().r)
 
   def CurrentDrivetrain(self):
-    if self.left_high:
-      if self.right_high:
+    if self.left_shifter_position > 0.5:
+      if self.right_shifter_position > 0.5:
         return self.drivetrain_high_high
       else:
         return self.drivetrain_high_low
     else:
-      if self.right_high:
+      if self.right_shifter_position > 0.5:
         return self.drivetrain_low_high
       else:
         return self.drivetrain_low_low
 
+  def SimShifter(self, gear, shifter_position):
+    if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+      shifter_position = min(shifter_position + 0.1, 1.0)
+    else:
+      shifter_position = max(shifter_position - 0.1, 0.0)
+
+    if shifter_position == 1.0:
+      gear = VelocityDrivetrain.HIGH
+    elif shifter_position == 0.0:
+      gear = VelocityDrivetrain.LOW
+
+    return gear, shifter_position
+
   def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
     high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
                   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"
+    #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) *
                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
     high_power = high_torque * high_omega
     low_power = low_torque * low_omega
-    if should_print:
-      print gear_name, "High omega", high_omega, "Low omega", low_omega
-      print gear_name, "High torque", high_torque, "Low torque", low_torque
-      print gear_name, "High power", high_power, "Low power", low_power
-      if (high_power > low_power) != current_gear:
-        if high_power > low_power:
-          print gear_name, "Shifting to high"
-        else:
-          print gear_name, "Shifting to low"
+    #if should_print:
+    #  print gear_name, "High omega", high_omega, "Low omega", low_omega
+    #  print gear_name, "High torque", high_torque, "Low torque", low_torque
+    #  print gear_name, "High power", high_power, "Low power", low_power
 
     # Shift algorithm improvements.
     # TODO(aschuh):
@@ -215,7 +244,23 @@
     # 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
+    goal_gear_is_high = high_power > low_power
+    #goal_gear_is_high = True
+
+    if not self.IsInGear(current_gear):
+      print gear_name, 'Not in gear.'
+      return current_gear
+    else:
+      is_high = current_gear is VelocityDrivetrain.HIGH
+      if is_high != goal_gear_is_high:
+        if goal_gear_is_high:
+          print gear_name, 'Shifting up.'
+          return VelocityDrivetrain.SHIFTING_UP
+        else:
+          print gear_name, 'Shifting down.'
+          return VelocityDrivetrain.SHIFTING_DOWN
+      else:
+        return current_gear
 
   def FilterVelocity(self, throttle):
     # Invert the plant to figure out how the velocity filter would have to work
@@ -255,65 +300,99 @@
     # This is the same as sending the most torque down to the floor at the
     # wheel.
 
-    self.left_high = self.ComputeGear(self.X[0, 0], should_print=True, current_gear=self.left_high, gear_name="left")
-    self.right_high = self.ComputeGear(self.X[1, 0], should_print=True, current_gear=self.right_high, gear_name="right")
+    self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+                                      current_gear=self.left_gear,
+                                      gear_name="left")
+    self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+                                       current_gear=self.right_gear,
+                                       gear_name="right")
+    if self.IsInGear(self.left_gear):
+      self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
 
-    FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+    if self.IsInGear(self.right_gear):
+      self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
 
-    # Filter the throttle to provide a nicer response.
+    if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+      # Filter the throttle to provide a nicer response.
+      fvel = self.FilterVelocity(throttle)
 
-    # TODO(austin): fn
-    fvel = self.FilterVelocity(throttle)
+      # Constant radius means that angualar_velocity / linear_velocity = constant.
+      # Compute the left and right velocities.
+      left_velocity = fvel - steering * numpy.abs(fvel)
+      right_velocity = fvel + steering * numpy.abs(fvel)
 
-    # Constant radius means that angualar_velocity / linear_velocity = constant.
-    # Compute the left and right velocities.
-    left_velocity = fvel - steering * numpy.abs(fvel)
-    right_velocity = fvel + steering * numpy.abs(fvel)
+      # Write this constraint in the form of K * R = w
+      # angular velocity / linear velocity = constant
+      # (left - right) / (left + right) = constant
+      # left - right = constant * left + constant * right
 
-    # Write this constraint in the form of K * R = w
-    # angular velocity / linear velocity = constant
-    # (left - right) / (left + right) = constant
-    # left - right = constant * left + constant * right
+      # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+      #  (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+      #       constant
+      # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+      # (-steering * sign(fvel)) = constant
+      # (-steering * sign(fvel)) * (left + right) = left - right
+      # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
 
-    # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
-    #  (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
-    #       constant
-    # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
-    # (-steering * sign(fvel)) = constant
-    # (-steering * sign(fvel)) * (left + right) = left - right
-    # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+      equality_k = numpy.matrix(
+          [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+      equality_w = 0.0
 
-    equality_k = numpy.matrix(
-        [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
-    equality_w = 0.0
+      self.R[0, 0] = left_velocity
+      self.R[1, 0] = right_velocity
 
-    self.R[0, 0] = left_velocity
-    self.R[1, 0] = right_velocity
+      # Construct a constraint on R by manipulating the constraint on U
+      # Start out with H * U <= k
+      # U = FF * R + K * (R - X)
+      # H * (FF * R + K * R - K * X) <= k
+      # H * (FF + K) * R <= k + H * K * X
+      R_poly = polytope.HPolytope(
+          self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+          self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
 
-    # Construct a constraint on R by manipulating the constraint on U
-    # Start out with H * U <= k
-    # U = FF * R + K * (R - X)
-    # H * (FF * R + K * R - K * X) <= k
-    # H * (FF + K) * R <= k + H * K * X
-    R_poly = polytope.HPolytope(
-        self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
-        self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+      # Limit R back inside the box.
+      self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
 
-    # Limit R back inside the box.
-    self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+      FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+      self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+    else:
+      print 'Not all in gear'
+      if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
+        # TODO(austin): Use battery volts here.
+        R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+        self.U_ideal[0, 0] = numpy.clip(
+            self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
+            self.left_cim.U_min, self.left_cim.U_max)
+        self.left_cim.Update(self.U_ideal[0, 0])
 
-    FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
-    self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+        R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
+        self.U_ideal[1, 0] = numpy.clip(
+            self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
+            self.right_cim.U_min, self.right_cim.U_max)
+        self.right_cim.Update(self.U_ideal[1, 0])
+      else:
+        assert False
 
     self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
-    self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+
+    # TODO(austin): Model the robot as not accelerating when you shift...
+    # This hack only works when you shift at the same time.
+    if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+      self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+
+    self.left_gear, self.left_shifter_position = self.SimShifter(
+        self.left_gear, self.left_shifter_position)
+    self.right_gear, self.right_shifter_position = self.SimShifter(
+        self.right_gear, self.right_shifter_position)
+
     print "U is", self.U[0, 0], self.U[1, 0]
+    print "Left shifter", self.left_gear, self.left_shifter_position, "Right shifter", self.right_gear, self.right_shifter_position
 
 
 def main(argv):
   vdrivetrain = VelocityDrivetrain()
 
-  if len(argv) != 3:
+  if len(argv) != 5:
     print "Expected .h file name and .cc file name"
   else:
     loop_writer = control_loop.ControlLoopWriter(
@@ -326,8 +405,15 @@
       loop_writer.Write(argv[2], argv[1])
     else:
       loop_writer.Write(argv[1], argv[2])
-    return
 
+    cim_writer = control_loop.ControlLoopWriter(
+        "CIM", [drivetrain.CIM()])
+
+    if argv[3][-3:] == '.cc':
+      cim_writer.Write(argv[4], argv[3])
+    else:
+      cim_writer.Write(argv[3], argv[4])
+    return
 
   vl_plot = []
   vr_plot = []
@@ -337,38 +423,40 @@
   t_plot = []
   left_gear_plot = []
   right_gear_plot = []
-  vdrivetrain.left_high = True
-  vdrivetrain.right_high = True
+  vdrivetrain.left_shifter_position = 0.0
+  vdrivetrain.right_shifter_position = 0.0
+  vdrivetrain.left_gear = VelocityDrivetrain.LOW
+  vdrivetrain.right_gear = VelocityDrivetrain.LOW
 
   print "K is", vdrivetrain.CurrentDrivetrain().K
 
-  if vdrivetrain.left_high:
+  if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
     print "Left is high"
   else:
     print "Left is low"
-  if vdrivetrain.right_high:
+  if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
     print "Right is high"
   else:
     print "Right is low"
 
-  for t in numpy.arange(0, 2.0, vdrivetrain.dt):
+  for t in numpy.arange(0, 4.0, vdrivetrain.dt):
     if t < 1.0:
-      vdrivetrain.Update(throttle=0.60, steering=0.3)
-    elif t < 1.5:
-      vdrivetrain.Update(throttle=0.60, steering=-0.3)
+      vdrivetrain.Update(throttle=0.60, steering=0.0)
+    elif t < 1.2:
+      vdrivetrain.Update(throttle=0.60, steering=0.0)
     else:
-      vdrivetrain.Update(throttle=0.0, steering=0.3)
+      vdrivetrain.Update(throttle=0.60, steering=0.0)
     t_plot.append(t)
     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)
+    left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+    right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.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:
+    if abs(fwd_velocity) < 0.0000001:
       radius_plot.append(turn_velocity)
     else:
       radius_plot.append(turn_velocity / fwd_velocity)
@@ -384,11 +472,16 @@
     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(cim_time, cim_velocity_plot, label='cim spinup')
+  #pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+  #pylab.legend()
+  #pylab.show()
 
+  # TODO(austin):
+  # Shifting compensation.
+
+  # Tighten the turn.
+  # Closed loop drive.
 
   pylab.plot(t_plot, vl_plot, label='left velocity')
   pylab.plot(t_plot, vr_plot, label='right velocity')