diff --git a/y2015/control_loops/python/BUILD b/y2015/control_loops/python/BUILD
index ab66d3e..77c3d81 100644
--- a/y2015/control_loops/python/BUILD
+++ b/y2015/control_loops/python/BUILD
@@ -1,6 +1,44 @@
 package(default_visibility = ['//y2015:__subpackages__'])
 
 py_binary(
+  name = 'drivetrain',
+  srcs = [
+    'drivetrain.py',
+  ],
+  deps = [
+    '//external:python-gflags',
+    '//external:python-glog',
+    '//frc971/control_loops/python:controls',
+  ],
+)
+
+py_binary(
+  name = 'polydrivetrain',
+  srcs = [
+    'polydrivetrain.py',
+    'drivetrain.py',
+  ],
+  deps = [
+    '//external:python-gflags',
+    '//external:python-glog',
+    '//frc971/control_loops/python:controls',
+  ],
+)
+
+py_library(
+  name = 'polydrivetrain_lib',
+  srcs = [
+    'polydrivetrain.py',
+    'drivetrain.py',
+  ],
+  deps = [
+    '//external:python-gflags',
+    '//external:python-glog',
+    '//frc971/control_loops/python:controls',
+  ],
+)
+
+py_binary(
   name = 'arm',
   srcs = [
     'arm.py',
diff --git a/y2015/control_loops/python/drivetrain.py b/y2015/control_loops/python/drivetrain.py
index 8ed62d6..75425ba 100755
--- a/y2015/control_loops/python/drivetrain.py
+++ b/y2015/control_loops/python/drivetrain.py
@@ -1,11 +1,17 @@
 #!/usr/bin/python
 
-import control_loop
-import controls
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
 import numpy
 import sys
 from matplotlib import pylab
 
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 
 class CIM(control_loop.ControlLoop):
   def __init__(self):
@@ -21,10 +27,10 @@
     # 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
+    self.resistance = 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))
+              (12.0 - self.resistance * self.free_current))
     # Torque constant
     self.Kt = self.stall_torque / self.stall_current
     # Control loop time step
@@ -32,9 +38,9 @@
 
     # State feedback matrices
     self.A_continuous = numpy.matrix(
-        [[-self.Kt / self.Kv / (self.J * self.R)]])
+        [[-self.Kt / self.Kv / (self.J * self.resistance)]])
     self.B_continuous = numpy.matrix(
-        [[self.Kt / (self.J * self.R)]])
+        [[self.Kt / (self.J * self.resistance)]])
     self.C = numpy.matrix([[1]])
     self.D = numpy.matrix([[0]])
 
@@ -53,14 +59,16 @@
 class Drivetrain(control_loop.ControlLoop):
   def __init__(self, name="Drivetrain", left_low=True, right_low=True):
     super(Drivetrain, self).__init__(name)
+    # Number of motors per side
+    self.num_motors = 1
     # Stall Torque in N m
-    self.stall_torque = 2.42
+    self.stall_torque = 2.42 * self.num_motors * 0.60
     # Stall Current in Amps
-    self.stall_current = 133.0
+    self.stall_current = 133.0 * self.num_motors
     # Free Speed in RPM. Used number from last year.
-    self.free_speed = 4650.0
+    self.free_speed = 5500.0
     # Free Current in Amps
-    self.free_current = 2.7
+    self.free_current = 4.7 * self.num_motors
     # Moment of inertia of the drivetrain in kg m^2
     # Just borrowed from last year.
     self.J = 10
@@ -71,10 +79,10 @@
     # Radius of the wheels, in meters.
     self.r = .0515938
     # Resistance of the motor, divided by the number of motors.
-    self.R = 12.0 / self.stall_current / 2
+    self.resistance = 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))
+               (12.0 - self.resistance * self.free_current))
     # Torque constant
     self.Kt = self.stall_torque / self.stall_current
     # Gear ratios
@@ -100,10 +108,10 @@
     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.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.R * self.r * self.r)
-    self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.R * self.r * self.r)
-    self.mpl = self.Kt / (self.Gl * self.R * self.r)
-    self.mpr = self.Kt / (self.Gr * self.R * self.r)
+    self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance * self.r * self.r)
+    self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance * self.r * self.r)
+    self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
+    self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
 
     # State feedback matrices
     # X will be of the format
@@ -123,17 +131,17 @@
     self.D = numpy.matrix([[0, 0],
                            [0, 0]])
 
-    #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.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.lp, self.hp, self.lp])
-    print self.K
-    q_pos = 0.07
-    q_vel = 1.0
+    if left_low or right_low:
+      q_pos = 0.12
+      q_vel = 1.0
+    else:
+      q_pos = 0.14
+      q_vel = 0.95
+
+    # Tune the LQR controller
     self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
                            [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
                            [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
@@ -142,10 +150,10 @@
     self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
                            [0.0, (1.0 / (12.0 ** 2.0))]])
     self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
-    print self.A
-    print self.B
-    print self.K
-    print numpy.linalg.eig(self.A - self.B * self.K)[0]
+
+    glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, name)
+    glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+    glog.debug('K %s', repr(self.K))
 
     self.hlp = 0.3
     self.llp = 0.4
@@ -153,11 +161,102 @@
 
     self.U_max = numpy.matrix([[12.0], [12.0]])
     self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
     self.InitializeState()
 
+
+class KFDrivetrain(Drivetrain):
+  def __init__(self, name="KFDrivetrain", left_low=True, right_low=True):
+    super(KFDrivetrain, self).__init__(name, left_low, right_low)
+
+    self.unaugmented_A_continuous = self.A_continuous
+    self.unaugmented_B_continuous = self.B_continuous
+
+    # The practical voltage applied to the wheels is
+    #   V_left = U_left + left_voltage_error
+    #
+    # The states are
+    # [left position, left velocity, right position, right velocity,
+    #  left voltage error, right voltage error, angular_error]
+    #
+    # The left and right positions are filtered encoder positions and are not
+    # adjusted for heading error.
+    # The turn velocity as computed by the left and right velocities is
+    # adjusted by the gyro velocity.
+    # The angular_error is the angular velocity error between the wheel speed
+    # and the gyro speed.
+    self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
+    self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
+    self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
+    self.A_continuous[0:4,4:6] = self.unaugmented_B_continuous
+    self.B_continuous[0:4,0:2] = self.unaugmented_B_continuous
+    self.A_continuous[0,6] = 1
+    self.A_continuous[2,6] = -1
+
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
+
+    self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
+                           [0, 0, 1, 0, 0, 0, 0],
+                           [0, -0.5 / self.rb, 0, 0.5 / self.rb, 0, 0, 0]])
+
+    self.D = numpy.matrix([[0, 0],
+                           [0, 0],
+                           [0, 0]])
+
+    q_pos = 0.05
+    q_vel = 1.00
+    q_voltage = 10.0
+    q_encoder_uncertainty = 2.00
+
+    self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+                           [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+                           [0.0, 0.0, (q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
+                           [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0, 0.0],
+                           [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
+                           [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty ** 2.0)]])
+
+    r_pos =  0.0001
+    r_gyro = 0.000001
+    self.R = numpy.matrix([[(r_pos ** 2.0), 0.0, 0.0],
+                           [0.0, (r_pos ** 2.0), 0.0],
+                           [0.0, 0.0, (r_gyro ** 2.0)]])
+
+    # Solving for kf gains.
+    self.KalmanGain, self.Q_steady = controls.kalman(
+        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+    self.L = self.A * self.KalmanGain
+
+    unaug_K = self.K
+
+    # Implement a nice closed loop controller for use by the closed loop
+    # controller.
+    self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+    self.K[0:2, 0:4] = unaug_K
+    self.K[0, 4] = 1.0
+    self.K[1, 5] = 1.0
+
+    self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+    qff_pos = 0.005
+    qff_vel = 1.00
+    self.Qff[0, 0] = 1.0 / qff_pos ** 2.0
+    self.Qff[1, 1] = 1.0 / qff_vel ** 2.0
+    self.Qff[2, 2] = 1.0 / qff_pos ** 2.0
+    self.Qff[3, 3] = 1.0 / qff_vel ** 2.0
+    self.Kff = numpy.matrix(numpy.zeros((2, 7)))
+    self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(self.B[0:4,:], self.Qff)
+
+    self.InitializeState()
+
+
 def main(argv):
+  argv = FLAGS(argv)
+  glog.init()
+
   # Simulate the response of the system to a step input.
-  drivetrain = Drivetrain()
+  drivetrain = Drivetrain(left_low=False, right_low=False)
   simulated_left = []
   simulated_right = []
   for _ in xrange(100):
@@ -165,26 +264,37 @@
     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()
+  if FLAGS.plot:
+    pylab.plot(range(100), simulated_left)
+    pylab.plot(range(100), simulated_right)
+    pylab.suptitle('Acceleration Test')
+    pylab.show()
 
   # Simulate forwards motion.
-  drivetrain = Drivetrain()
+  drivetrain = Drivetrain(left_low=False, right_low=False)
   close_loop_left = []
   close_loop_right = []
+  left_power = []
+  right_power = []
   R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
-  for _ in xrange(100):
+  for _ in xrange(300):
     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])
+    left_power.append(U[0, 0])
+    right_power.append(U[1, 0])
 
-  #pylab.plot(range(100), close_loop_left)
-  #pylab.plot(range(100), close_loop_right)
-  #pylab.show()
+  if FLAGS.plot:
+    pylab.plot(range(300), close_loop_left, label='left position')
+    pylab.plot(range(300), close_loop_right, label='right position')
+    pylab.plot(range(300), left_power, label='left power')
+    pylab.plot(range(300), right_power, label='right power')
+    pylab.suptitle('Linear Move')
+    pylab.legend()
+    pylab.show()
 
   # Try turning in place
   drivetrain = Drivetrain()
@@ -199,9 +309,11 @@
     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()
+  if FLAGS.plot:
+    pylab.plot(range(100), close_loop_left)
+    pylab.plot(range(100), close_loop_right)
+    pylab.suptitle('Angular Move')
+    pylab.show()
 
   # Try turning just one side.
   drivetrain = Drivetrain()
@@ -216,27 +328,75 @@
     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()
+  if FLAGS.plot:
+    pylab.plot(range(100), close_loop_left)
+    pylab.plot(range(100), close_loop_right)
+    pylab.suptitle('Pivot')
+    pylab.show()
 
   # Write the generated constants out to a file.
-  print "Output one"
-  drivetrain_low_low = Drivetrain(name="DrivetrainLowLow", left_low=True, right_low=True)
-  drivetrain_low_high = Drivetrain(name="DrivetrainLowHigh", left_low=True, right_low=False)
-  drivetrain_high_low = Drivetrain(name="DrivetrainHighLow", left_low=False, right_low=True)
-  drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh", left_low=False, right_low=False)
+  drivetrain_low_low = Drivetrain(
+      name="DrivetrainLowLow", left_low=True, right_low=True)
+  drivetrain_low_high = Drivetrain(
+      name="DrivetrainLowHigh", left_low=True, right_low=False)
+  drivetrain_high_low = Drivetrain(
+      name="DrivetrainHighLow", left_low=False, right_low=True)
+  drivetrain_high_high = Drivetrain(
+      name="DrivetrainHighHigh", left_low=False, right_low=False)
+
+  kf_drivetrain_low_low = KFDrivetrain(
+      name="KFDrivetrainLowLow", left_low=True, right_low=True)
+  kf_drivetrain_low_high = KFDrivetrain(
+      name="KFDrivetrainLowHigh", left_low=True, right_low=False)
+  kf_drivetrain_high_low = KFDrivetrain(
+      name="KFDrivetrainHighLow", left_low=False, right_low=True)
+  kf_drivetrain_high_high = KFDrivetrain(
+      name="KFDrivetrainHighHigh", left_low=False, right_low=False)
 
   if len(argv) != 5:
     print "Expected .h file name and .cc file name"
   else:
+    namespaces = ['y2015', 'control_loops', 'drivetrain']
     dog_loop_writer = control_loop.ControlLoopWriter(
         "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
-                       drivetrain_high_low, drivetrain_high_high])
-    if argv[1][-3:] == '.cc':
-      dog_loop_writer.Write(argv[2], argv[1])
-    else:
-      dog_loop_writer.Write(argv[1], argv[2])
+                       drivetrain_high_low, drivetrain_high_high],
+        namespaces = namespaces)
+    dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+          drivetrain_low_low.dt))
+    dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
+          drivetrain_low_low.stall_torque))
+    dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
+          drivetrain_low_low.stall_current))
+    dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeedRPM", "%f",
+          drivetrain_low_low.free_speed))
+    dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
+          drivetrain_low_low.free_current))
+    dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
+          drivetrain_low_low.J))
+    dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
+          drivetrain_low_low.m))
+    dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
+          drivetrain_low_low.rb))
+    dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
+          drivetrain_low_low.r))
+    dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
+          drivetrain_low_low.resistance))
+    dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
+          drivetrain_low_low.Kv))
+    dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
+          drivetrain_low_low.Kt))
+    dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
+          drivetrain_low_low.G_low))
+    dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
+          drivetrain_high_high.G_high))
+
+    dog_loop_writer.Write(argv[1], argv[2])
+
+    kf_loop_writer = control_loop.ControlLoopWriter(
+        "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
+                         kf_drivetrain_high_low, kf_drivetrain_high_high],
+        namespaces = namespaces)
+    kf_loop_writer.Write(argv[3], argv[4])
 
 if __name__ == '__main__':
   sys.exit(main(sys.argv))
diff --git a/y2015/control_loops/python/polydrivetrain.py b/y2015/control_loops/python/polydrivetrain.py
index 62e7154..5320145 100755
--- a/y2015/control_loops/python/polydrivetrain.py
+++ b/y2015/control_loops/python/polydrivetrain.py
@@ -2,14 +2,23 @@
 
 import numpy
 import sys
-import polytope
-import drivetrain
-import control_loop
-import controls
+from frc971.control_loops.python import polytope
+from y2015.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
 from matplotlib import pylab
 
+import gflags
+import glog
+
 __author__ = 'Austin Schuh (austin.linux@gmail.com)'
 
+FLAGS = gflags.FLAGS
+
+try:
+  gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+  pass
 
 def CoerceGoal(region, K, w, R):
   """Intersects a line with a region, and finds the closest point to R.
@@ -37,7 +46,7 @@
   perpendicular_vector = K.T / numpy.linalg.norm(K)
   parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
                                   [-perpendicular_vector[0, 0]]])
-  
+
   # We want to impose the constraint K * X = w on the polytope H * X <= k.
   # We do this by breaking X up into parallel and perpendicular components to
   # the half plane.  This gives us the following equation.
@@ -124,7 +133,7 @@
 
     self.G_high = self._drivetrain.G_high
     self.G_low = self._drivetrain.G_low
-    self.R = self._drivetrain.R
+    self.resistance = self._drivetrain.resistance
     self.r = self._drivetrain.r
     self.Kv = self._drivetrain.Kv
     self.Kt = self._drivetrain.Kt
@@ -174,6 +183,10 @@
         [[0.0],
          [0.0]])
 
+    self.U_ideal = numpy.matrix(
+        [[0.0],
+         [0.0]])
+
     # ttrust is the comprimise between having full throttle negative inertia,
     # and having no throttle negative inertia.  A value of 0 is full throttle
     # inertia.  A value of 1 is no throttle negative inertia.
@@ -229,9 +242,9 @@
                  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)
+                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
     low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
-                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().R)
+                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
     high_power = high_torque * high_omega
     low_power = low_torque * low_omega
     #if should_print:
@@ -250,16 +263,16 @@
     #goal_gear_is_high = True
 
     if not self.IsInGear(current_gear):
-      print gear_name, 'Not in gear.'
+      glog.debug('%s Not in gear.', gear_name)
       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.'
+          glog.debug('%s Shifting up.', gear_name)
           return VelocityDrivetrain.SHIFTING_UP
         else:
-          print gear_name, 'Shifting down.'
+          glog.debug('%s Shifting down.', gear_name)
           return VelocityDrivetrain.SHIFTING_DOWN
       else:
         return current_gear
@@ -362,7 +375,7 @@
       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'
+      glog.debug('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])
@@ -391,35 +404,36 @@
     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
+    glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+    glog.debug('Left shifter %s %d Right shifter %s %d',
+               self.left_gear, self.left_shifter_position,
+               self.right_gear, self.right_shifter_position)
 
 
 def main(argv):
+  argv = FLAGS(argv)
+
   vdrivetrain = VelocityDrivetrain()
 
-  if len(argv) != 7:
-    print "Expected .h file name and .cc file name"
-  else:
-    dog_loop_writer = control_loop.ControlLoopWriter(
-        "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
-                       vdrivetrain.drivetrain_low_high,
-                       vdrivetrain.drivetrain_high_low,
-                       vdrivetrain.drivetrain_high_high])
-
-    if argv[1][-3:] == '.cc':
-      dog_loop_writer.Write(argv[2], argv[1])
+  if not FLAGS.plot:
+    if len(argv) != 5:
+      glog.fatal('Expected .h file name and .cc file name')
     else:
+      namespaces = ['y2015', 'control_loops', 'drivetrain']
+      dog_loop_writer = control_loop.ControlLoopWriter(
+          "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+                         vdrivetrain.drivetrain_low_high,
+                         vdrivetrain.drivetrain_high_low,
+                         vdrivetrain.drivetrain_high_high],
+                         namespaces=namespaces)
+
       dog_loop_writer.Write(argv[1], argv[2])
 
-    cim_writer = control_loop.ControlLoopWriter(
-        "CIM", [drivetrain.CIM()])
+      cim_writer = control_loop.ControlLoopWriter(
+          "CIM", [drivetrain.CIM()])
 
-    if argv[5][-3:] == '.cc':
-      cim_writer.Write(argv[6], argv[5])
-    else:
-      cim_writer.Write(argv[5], argv[6])
-    return
+      cim_writer.Write(argv[3], argv[4])
+      return
 
   vl_plot = []
   vr_plot = []
@@ -434,16 +448,16 @@
   vdrivetrain.left_gear = VelocityDrivetrain.LOW
   vdrivetrain.right_gear = VelocityDrivetrain.LOW
 
-  print "K is", vdrivetrain.CurrentDrivetrain().K
+  glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
 
   if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
-    print "Left is high"
+    glog.debug('Left is high')
   else:
-    print "Left is low"
+    glog.debug('Left is low')
   if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
-    print "Right is high"
+    glog.debug('Right is high')
   else:
-    print "Right is low"
+    glog.debug('Right is low')
 
   for t in numpy.arange(0, 1.7, vdrivetrain.dt):
     if t < 0.5: