diff --git a/y2018/control_loops/python/arm_distal.py b/y2018/control_loops/python/arm_distal.py
index 2836c50..d443f8a 100755
--- a/y2018/control_loops/python/arm_distal.py
+++ b/y2018/control_loops/python/arm_distal.py
@@ -3,7 +3,7 @@
 # This code was used to select the gear ratio for the distal arm.
 # Run it from the command line and it displays the time required
 # to move the distal arm 60 degrees.
-# 
+#
 # Michael Schuh
 # January 20, 2018
 
@@ -14,206 +14,238 @@
 # apt-get install python-scipy python3-scipy python-numpy python3-numpy
 
 pi = math.pi
-pi2 = 2.0*pi
-rad_to_deg = 180.0/pi
+pi2 = 2.0 * pi
+rad_to_deg = 180.0 / pi
 inches_to_meters = 0.0254
-lbs_to_kg = 1.0/2.2
+lbs_to_kg = 1.0 / 2.2
 newton_to_lbf = 0.224809
 newton_meters_to_ft_lbs = 0.73756
 run_count = 0
 theta_travel = 0.0
 
+
 def to_deg(angle):
-  return (angle*rad_to_deg)
+    return (angle * rad_to_deg)
+
 
 def to_rad(angle):
-  return (angle/rad_to_deg)
+    return (angle / rad_to_deg)
+
 
 def to_rotations(angle):
-  return (angle/pi2)
+    return (angle / pi2)
+
 
 def time_derivative(x, t, voltage, c1, c2, c3):
-  global run_count
-  theta, omega = x
-  dxdt = [omega, -c1*omega + c3*math.sin(theta) + c2*voltage]
-  run_count = run_count + 1
+    global run_count
+    theta, omega = x
+    dxdt = [omega, -c1 * omega + c3 * math.sin(theta) + c2 * voltage]
+    run_count = run_count + 1
 
-  #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
-  return dxdt
-  
+    #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
+    return dxdt
 
-def get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed):
-  #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
-  #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
-  global run_count
-  global theta_travel
 
-  if ( False ):
-    # Gravity is assisting the motion.
-    theta_start = 0.0
-    theta_target = pi
-  elif ( False ):
-    # Gravity is assisting the motion.
-    theta_start = 0.0
-    theta_target = -pi
-  elif ( False ):
-    # Gravity is slowing the motion.
-    theta_start = pi
-    theta_target = 0.0
-  elif ( True ):
-    # Gravity is slowing the motion.
-    theta_start = -pi
-    theta_target = 0.0
+def get_180_degree_time(c1, c2, c3, voltage, gear_ratio, motor_free_speed):
+    #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
+    #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
+    global run_count
+    global theta_travel
 
-  theta_half = 0.5*(theta_start + theta_target)
-  if (theta_start > theta_target):
-    voltage = -voltage
-  theta = theta_start
-  theta_travel = theta_start - theta_target 
-  if ( run_count == 0 ):
-    print ("# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (theta_start,theta_target,theta_travel,theta_half, voltage))
-    print ("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (to_deg(theta_start),to_deg(theta_target),to_deg(theta_travel),to_deg(theta_half), voltage))
-  omega = 0.0
-  time = 0.0
-  delta_time = 0.01 # time step in seconds
-  for step in range(1, 5000):
-     t = numpy.array([time, time + delta_time])
-     time = time + delta_time
-     x = [theta, omega]
-     angular_acceleration = -c1*omega + c2*voltage
-     x_n_plus_1 = scipy.integrate.odeint(time_derivative,x,t,args=(voltage,c1,c2,c3))
-     #print ('x_n_plus_1 = ',x_n_plus_1)
-     #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
-     theta, omega = x_n_plus_1[1]
-     #theta= x_n_plus_1[0]
-     #omega = x_n_plus_1[1]
-     if ( False ):
-       print ("%4d  %8.4f %8.2f          %8.4f          %8.4f    %8.3f    %8.3f     %8.3f      %8.3f" % \
-         (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
-         to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
-     if (theta_start < theta_target):
-       # Angle is increasing through the motion.
-       if (theta > theta_half):
-         break
-     else:
-       # Angle is decreasing through the motion.
-       if (theta < theta_half):
-         break
-       
-  #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
-  #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
-  #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
-  return (2.0*time)
+    if (False):
+        # Gravity is assisting the motion.
+        theta_start = 0.0
+        theta_target = pi
+    elif (False):
+        # Gravity is assisting the motion.
+        theta_start = 0.0
+        theta_target = -pi
+    elif (False):
+        # Gravity is slowing the motion.
+        theta_start = pi
+        theta_target = 0.0
+    elif (True):
+        # Gravity is slowing the motion.
+        theta_start = -pi
+        theta_target = 0.0
+
+    theta_half = 0.5 * (theta_start + theta_target)
+    if (theta_start > theta_target):
+        voltage = -voltage
+    theta = theta_start
+    theta_travel = theta_start - theta_target
+    if (run_count == 0):
+        print(
+            "# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f"
+            % (theta_start, theta_target, theta_travel, theta_half, voltage))
+        print(
+            "# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f"
+            % (to_deg(theta_start), to_deg(theta_target), to_deg(theta_travel),
+               to_deg(theta_half), voltage))
+    omega = 0.0
+    time = 0.0
+    delta_time = 0.01  # time step in seconds
+    for step in range(1, 5000):
+        t = numpy.array([time, time + delta_time])
+        time = time + delta_time
+        x = [theta, omega]
+        angular_acceleration = -c1 * omega + c2 * voltage
+        x_n_plus_1 = scipy.integrate.odeint(time_derivative,
+                                            x,
+                                            t,
+                                            args=(voltage, c1, c2, c3))
+        #print ('x_n_plus_1 = ',x_n_plus_1)
+        #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
+        theta, omega = x_n_plus_1[1]
+        #theta= x_n_plus_1[0]
+        #omega = x_n_plus_1[1]
+        if (False):
+            print ("%4d  %8.4f %8.2f          %8.4f          %8.4f    %8.3f    %8.3f     %8.3f      %8.3f" % \
+              (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
+              to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
+        if (theta_start < theta_target):
+            # Angle is increasing through the motion.
+            if (theta > theta_half):
+                break
+        else:
+            # Angle is decreasing through the motion.
+            if (theta < theta_half):
+                break
+
+    #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
+    #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
+    #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
+    return (2.0 * time)
+
 
 def main():
-  gravity = 9.8 # m/sec^2 Gravity Constant
-  voltage_nominal = 12 # Volts
-  
-  # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
-  motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
-  current_stall = 134 # amps stall current
-  current_no_load = 0.7 # amps no load current
-  torque_stall = 710/1000.0 # N-m Stall Torque
-  speed_no_load_rpm = 18730 # RPM no load speed
-  
-  if ( False ):
-    # Bag motor from https://www.vexrobotics.com/217-3351.html
-    motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
-    current_stall = 53.0 # amps stall current
-    current_no_load = 1.8 # amps no load current
-    torque_stall = 0.4 # N-m Stall Torque
-    speed_no_load_rpm = 13180.0 # RPM no load speed
-  
-  if ( True ):
-    # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
-    motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
-    current_stall = 89.0 # amps stall current
-    current_no_load = 3.0 # amps no load current
-    torque_stall = 1.4 # N-m Stall Torque
-    speed_no_load_rpm = 5840.0 # RPM no load speed
+    gravity = 9.8  # m/sec^2 Gravity Constant
+    voltage_nominal = 12  # Volts
 
-  # How many motors are we using?
-  num_motors = 2
+    # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
+    motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
+    current_stall = 134  # amps stall current
+    current_no_load = 0.7  # amps no load current
+    torque_stall = 710 / 1000.0  # N-m Stall Torque
+    speed_no_load_rpm = 18730  # RPM no load speed
 
-  # Motor values
-  print ("# Motor: %s" % (motor_name))
-  print ("# Number of motors: %d" % (num_motors))
-  print ("# Stall torque: %.1f n-m" % (torque_stall))
-  print ("# Stall current: %.1f amps" % (current_stall))
-  print ("# No load current: %.1f amps" % (current_no_load))a
-  print ("# No load speed: %.0f rpm" % (speed_no_load_rpm))
-  
-  # Constants from motor values
-  resistance_motor = voltage_nominal/current_stall 
-  speed_no_load_rps = speed_no_load_rpm/60.0 # Revolutions per second no load speed
-  speed_no_load = speed_no_load_rps*2.0*pi
-  Kt = num_motors*torque_stall/current_stall # N-m/A torque constant
-  Kv_rpm = speed_no_load_rpm /(voltage_nominal - resistance_motor*current_no_load)  # rpm/V
-  Kv = Kv_rpm*2.0*pi/60.0 # rpm/V
-  
-  # Robot Geometry and physics
-  length_proximal_arm = inches_to_meters*47.34 # m Length of arm connected to the robot base
-  length_distal_arm = inches_to_meters*44.0 # m Length of arm that holds the cube
-  mass_cube = 6.0*lbs_to_kg  # Weight of the cube in Kgrams
-  mass_proximal_arm = 5.5*lbs_to_kg # Weight of proximal arm
-  mass_distal_arm = 3.5*lbs_to_kg # Weight of distal arm
-  mass_distal = mass_cube + mass_distal_arm
-  radius_to_proximal_arm_cg = 22.0*inches_to_meters # m Length from arm pivot point to arm CG
-  radius_to_distal_arm_cg = 10.0*inches_to_meters # m Length from arm pivot point to arm CG
+    if (False):
+        # Bag motor from https://www.vexrobotics.com/217-3351.html
+        motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
+        current_stall = 53.0  # amps stall current
+        current_no_load = 1.8  # amps no load current
+        torque_stall = 0.4  # N-m Stall Torque
+        speed_no_load_rpm = 13180.0  # RPM no load speed
 
-  radius_to_distal_cg = ( length_distal_arm*mass_cube + radius_to_distal_arm_cg*mass_distal_arm)/mass_distal
-  J_cube = length_distal_arm*length_distal_arm*mass_cube 
-  J_proximal_arm = radius_to_proximal_arm_cg*radius_to_proximal_arm_cg*mass_distal_arm # Kg m^2 Moment of inertia of the proximal arm
-  J_distal_arm = radius_to_distal_arm_cg*radius_to_distal_arm_cg*mass_distal_arm # Kg m^2 Moment of inertia of the distal arm
-  J = J_cube + J_distal_arm # Moment of inertia of the arm with the cube on the end
+    if (True):
+        # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
+        motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
+        current_stall = 89.0  # amps stall current
+        current_no_load = 3.0  # amps no load current
+        torque_stall = 1.4  # N-m Stall Torque
+        speed_no_load_rpm = 5840.0  # RPM no load speed
 
-  error_margine = 1.0
-  voltage = 10.0 # voltage for the motor.  Assuming a loaded robot so not using 12 V.
-  # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
-  # motor_free_speed = Kv*voltage
-  motor_free_speed = speed_no_load
-  
-  
-  print ("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" % (Kt, Kv_rpm, Kv))
-  print ("# %.2f Ohms Resistance of the motor " % (resistance_motor))
-  print ("# %.2f kg Cube weight" % (mass_cube))
-  print ("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
-  print ("# %.2f kg Distal Arm mass" % (mass_distal_arm))
-  print ("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
-  print ("# %.2f m Length from distal arm pivot point to arm CG" % (radius_to_distal_arm_cg))
-  print ("# %.2f m Length from distal arm pivot point to arm and cube cg" % (radius_to_distal_cg))
-  print ("# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point" % (J_cube))
-  print ("# %.2f kg-m^2 Moment of inertia of the arm with the cube on the end" % (J))
-  print ("# %.2f m  Proximal arm length" % (length_proximal_arm))
-  print ("# %.2f m  Distal arm length" % (length_distal_arm))
+    # How many motors are we using?
+    num_motors = 2
 
-  print ("# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point" % (J_proximal_arm))
-  print ("# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point" % (J_distal_arm))
-  print ("# %.2f kg-m^2 Moment of inertia of the distal arm and cube about the arm pivot point" % (J))
-  print ("# %d Number of motors" % (num_motors))
-  
-  print ("# %.2f V Motor voltage" % (voltage))
-  for gear_ratio in range(30, 181, 10):
-    c1 = Kt*gear_ratio*gear_ratio/(Kv*resistance_motor*J)
-    c2 = gear_ratio*Kt/(J*resistance_motor)
-    c3 = radius_to_distal_cg*mass_distal*gravity/J
-  
-    if ( False ):
-      print ("# %.8f 1/sec C1 constant" % (c1))
-      print ("# %.2f 1/sec C2 constant" % (c2))
-      print ("# %.2f 1/(V sec^2) C3 constant" % (c3))
-      print ("# %.2f RPM Free speed at motor voltage" % (voltage*Kv_rpm))
-  
-    torque_90_degrees = radius_to_distal_cg*mass_distal*gravity
-    voltage_90_degrees = resistance_motor*torque_90_degrees/(gear_ratio*Kt)
-    torque_peak = gear_ratio*num_motors*torque_stall
-    torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
-    normal_force = torque_peak/length_distal_arm
-    normal_force_lbf = newton_to_lbf*normal_force 
-    time_required = get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed)
-    print ("Time for %.1f degrees for gear ratio %3.0f is %.2f seconds.  90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at distal end %3.0f N %2.0f lbf" % \
-      (to_deg(theta_travel),gear_ratio,time_required,torque_90_degrees,voltage_90_degrees,
-       torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf))
-  
+    # Motor values
+    print("# Motor: %s" % (motor_name))
+    print("# Number of motors: %d" % (num_motors))
+    print("# Stall torque: %.1f n-m" % (torque_stall))
+    print("# Stall current: %.1f amps" % (current_stall))
+    print("# No load current: %.1f amps" % (current_no_load))
+    print("# No load speed: %.0f rpm" % (speed_no_load_rpm))
+
+    # Constants from motor values
+    resistance_motor = voltage_nominal / current_stall
+    speed_no_load_rps = speed_no_load_rpm / 60.0  # Revolutions per second no load speed
+    speed_no_load = speed_no_load_rps * 2.0 * pi
+    Kt = num_motors * torque_stall / current_stall  # N-m/A torque constant
+    Kv_rpm = speed_no_load_rpm / (
+        voltage_nominal - resistance_motor * current_no_load)  # rpm/V
+    Kv = Kv_rpm * 2.0 * pi / 60.0  # rpm/V
+
+    # Robot Geometry and physics
+    length_proximal_arm = inches_to_meters * 47.34  # m Length of arm connected to the robot base
+    length_distal_arm = inches_to_meters * 44.0  # m Length of arm that holds the cube
+    mass_cube = 6.0 * lbs_to_kg  # Weight of the cube in Kgrams
+    mass_proximal_arm = 5.5 * lbs_to_kg  # Weight of proximal arm
+    mass_distal_arm = 3.5 * lbs_to_kg  # Weight of distal arm
+    mass_distal = mass_cube + mass_distal_arm
+    radius_to_proximal_arm_cg = 22.0 * inches_to_meters  # m Length from arm pivot point to arm CG
+    radius_to_distal_arm_cg = 10.0 * inches_to_meters  # m Length from arm pivot point to arm CG
+
+    radius_to_distal_cg = (
+        length_distal_arm * mass_cube +
+        radius_to_distal_arm_cg * mass_distal_arm) / mass_distal
+    J_cube = length_distal_arm * length_distal_arm * mass_cube
+    J_proximal_arm = radius_to_proximal_arm_cg * radius_to_proximal_arm_cg * mass_distal_arm  # Kg m^2 Moment of inertia of the proximal arm
+    J_distal_arm = radius_to_distal_arm_cg * radius_to_distal_arm_cg * mass_distal_arm  # Kg m^2 Moment of inertia of the distal arm
+    J = J_cube + J_distal_arm  # Moment of inertia of the arm with the cube on the end
+
+    error_margine = 1.0
+    voltage = 10.0  # voltage for the motor.  Assuming a loaded robot so not using 12 V.
+    # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
+    # motor_free_speed = Kv*voltage
+    motor_free_speed = speed_no_load
+
+    print("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" %
+          (Kt, Kv_rpm, Kv))
+    print("# %.2f Ohms Resistance of the motor " % (resistance_motor))
+    print("# %.2f kg Cube weight" % (mass_cube))
+    print("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
+    print("# %.2f kg Distal Arm mass" % (mass_distal_arm))
+    print("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
+    print("# %.2f m Length from distal arm pivot point to arm CG" %
+          (radius_to_distal_arm_cg))
+    print("# %.2f m Length from distal arm pivot point to arm and cube cg" %
+          (radius_to_distal_cg))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point"
+        % (J_cube))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the arm with the cube on the end" %
+        (J))
+    print("# %.2f m  Proximal arm length" % (length_proximal_arm))
+    print("# %.2f m  Distal arm length" % (length_distal_arm))
+
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point"
+        % (J_proximal_arm))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point"
+        % (J_distal_arm))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the distal arm and cube about the arm pivot point"
+        % (J))
+    print("# %d Number of motors" % (num_motors))
+
+    print("# %.2f V Motor voltage" % (voltage))
+    for gear_ratio in range(30, 181, 10):
+        c1 = Kt * gear_ratio * gear_ratio / (Kv * resistance_motor * J)
+        c2 = gear_ratio * Kt / (J * resistance_motor)
+        c3 = radius_to_distal_cg * mass_distal * gravity / J
+
+        if (False):
+            print("# %.8f 1/sec C1 constant" % (c1))
+            print("# %.2f 1/sec C2 constant" % (c2))
+            print("# %.2f 1/(V sec^2) C3 constant" % (c3))
+            print("# %.2f RPM Free speed at motor voltage" %
+                  (voltage * Kv_rpm))
+
+        torque_90_degrees = radius_to_distal_cg * mass_distal * gravity
+        voltage_90_degrees = resistance_motor * torque_90_degrees / (
+            gear_ratio * Kt)
+        torque_peak = gear_ratio * num_motors * torque_stall
+        torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
+        normal_force = torque_peak / length_distal_arm
+        normal_force_lbf = newton_to_lbf * normal_force
+        time_required = get_180_degree_time(c1, c2, c3, voltage, gear_ratio,
+                                            motor_free_speed)
+        print ("Time for %.1f degrees for gear ratio %3.0f is %.2f seconds.  90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at distal end %3.0f N %2.0f lbf" % \
+          (to_deg(theta_travel),gear_ratio,time_required,torque_90_degrees,voltage_90_degrees,
+           torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf))
+
+
 if __name__ == '__main__':
-   main()
+    main()
diff --git a/y2018/control_loops/python/arm_proximal.py b/y2018/control_loops/python/arm_proximal.py
index d40ec8a..4abd1cf 100755
--- a/y2018/control_loops/python/arm_proximal.py
+++ b/y2018/control_loops/python/arm_proximal.py
@@ -3,7 +3,7 @@
 # This code was used to select the gear ratio for the proximal arm.
 # Run it from the command line and it displays the time required
 # to move the proximal arm 180 degrees from straight down to straight up.
-# 
+#
 # Michael Schuh
 # January 20, 2018
 
@@ -14,241 +14,291 @@
 # apt-get install python-scipy python3-scipy python-numpy python3-numpy
 
 pi = math.pi
-pi2 = 2.0*pi
-rad_to_deg = 180.0/pi
+pi2 = 2.0 * pi
+rad_to_deg = 180.0 / pi
 inches_to_meters = 0.0254
-lbs_to_kg = 1.0/2.2
+lbs_to_kg = 1.0 / 2.2
 newton_to_lbf = 0.224809
 newton_meters_to_ft_lbs = 0.73756
 run_count = 0
 theta_travel = 0.0
 
+
 def to_deg(angle):
-  return (angle*rad_to_deg)
+    return (angle * rad_to_deg)
+
 
 def to_rad(angle):
-  return (angle/rad_to_deg)
+    return (angle / rad_to_deg)
+
 
 def to_rotations(angle):
-  return (angle/pi2)
+    return (angle / pi2)
+
 
 def time_derivative(x, t, voltage, c1, c2, c3):
-  global run_count
-  theta, omega = x
-  dxdt = [omega, -c1*omega + c3*math.sin(theta) + c2*voltage]
-  run_count = run_count + 1
+    global run_count
+    theta, omega = x
+    dxdt = [omega, -c1 * omega + c3 * math.sin(theta) + c2 * voltage]
+    run_count = run_count + 1
 
-  #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
-  return dxdt
+    #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
+    return dxdt
+
 
 def get_distal_angle(theta_proximal):
-  # For the proximal angle = -50 degrees, the distal angle is -180 degrees
-  # For the proximal angle =  10 degrees, the distal angle is  -90 degrees
-  distal_angle = to_rad(-180.0 - (-50.0-to_deg(theta_proximal))*(180.0-90.0)/(50.0+10.0))
-  return distal_angle
+    # For the proximal angle = -50 degrees, the distal angle is -180 degrees
+    # For the proximal angle =  10 degrees, the distal angle is  -90 degrees
+    distal_angle = to_rad(-180.0 - (-50.0 - to_deg(theta_proximal)) *
+                          (180.0 - 90.0) / (50.0 + 10.0))
+    return distal_angle
+
 
 def get_distal_omega(omega_proximal):
-  # For the proximal angle = -50 degrees, the distal angle is -180 degrees
-  # For the proximal angle =  10 degrees, the distal angle is  -90 degrees
-  distal_angle = omega_proximal*( (180.0-90.0) / (50.0+10.0) )
-  return distal_angle
-  
+    # For the proximal angle = -50 degrees, the distal angle is -180 degrees
+    # For the proximal angle =  10 degrees, the distal angle is  -90 degrees
+    distal_angle = omega_proximal * ((180.0 - 90.0) / (50.0 + 10.0))
+    return distal_angle
 
-def get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed):
-  #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
-  #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
-  global run_count
-  global theta_travel
 
-  if ( False ):
-    # Gravity is assisting the motion.
-    theta_start = 0.0
-    theta_target = pi
-  elif ( False ):
-    # Gravity is assisting the motion.
-    theta_start = 0.0
-    theta_target = -pi
-  elif ( False ):
-    # Gravity is slowing the motion.
-    theta_start = pi
-    theta_target = 0.0
-  elif ( False ):
-    # Gravity is slowing the motion.
-    theta_start = -pi
-    theta_target = 0.0
-  elif ( True ):
-    # This is for the proximal arm motion.
-    theta_start = to_rad(-50.0)
-    theta_target = to_rad(10.0)
+def get_180_degree_time(c1, c2, c3, voltage, gear_ratio, motor_free_speed):
+    #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
+    #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
+    global run_count
+    global theta_travel
 
-  theta_half = 0.5*(theta_start + theta_target)
-  if (theta_start > theta_target):
-    voltage = -voltage
-  theta = theta_start
-  theta_travel = theta_start - theta_target 
-  if ( run_count == 0 ):
-    print ("# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (theta_start,theta_target,theta_travel,theta_half, voltage))
-    print ("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (to_deg(theta_start),to_deg(theta_target),to_deg(theta_travel),to_deg(theta_half), voltage))
-  omega = 0.0
-  time = 0.0
-  delta_time = 0.01 # time step in seconds
-  for step in range(1, 5000):
-     t = numpy.array([time, time + delta_time])
-     time = time + delta_time
-     x = [theta, omega]
-     angular_acceleration = -c1*omega + c2*voltage
-     x_n_plus_1 = scipy.integrate.odeint(time_derivative,x,t,args=(voltage,c1,c2,c3))
-     #print ('x_n_plus_1 = ',x_n_plus_1)
-     #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
-     theta, omega = x_n_plus_1[1]
-     #theta= x_n_plus_1[0]
-     #omega = x_n_plus_1[1]
-     if ( False ):
-       print ("%4d  %8.4f %8.2f          %8.4f          %8.4f    %8.3f    %8.3f     %8.3f      %8.3f" % \
-         (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
-         to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
-     if (theta_start < theta_target):
-       # Angle is increasing through the motion.
-       if (theta > theta_half):
-         break
-     else:
-       # Angle is decreasing through the motion.
-       if (theta < theta_half):
-         break
-       
-  #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
-  #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
-  #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
-  return (2.0*time)
+    if (False):
+        # Gravity is assisting the motion.
+        theta_start = 0.0
+        theta_target = pi
+    elif (False):
+        # Gravity is assisting the motion.
+        theta_start = 0.0
+        theta_target = -pi
+    elif (False):
+        # Gravity is slowing the motion.
+        theta_start = pi
+        theta_target = 0.0
+    elif (False):
+        # Gravity is slowing the motion.
+        theta_start = -pi
+        theta_target = 0.0
+    elif (True):
+        # This is for the proximal arm motion.
+        theta_start = to_rad(-50.0)
+        theta_target = to_rad(10.0)
+
+    theta_half = 0.5 * (theta_start + theta_target)
+    if (theta_start > theta_target):
+        voltage = -voltage
+    theta = theta_start
+    theta_travel = theta_start - theta_target
+    if (run_count == 0):
+        print(
+            "# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f"
+            % (theta_start, theta_target, theta_travel, theta_half, voltage))
+        print(
+            "# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f"
+            % (to_deg(theta_start), to_deg(theta_target), to_deg(theta_travel),
+               to_deg(theta_half), voltage))
+    omega = 0.0
+    time = 0.0
+    delta_time = 0.01  # time step in seconds
+    for step in range(1, 5000):
+        t = numpy.array([time, time + delta_time])
+        time = time + delta_time
+        x = [theta, omega]
+        angular_acceleration = -c1 * omega + c2 * voltage
+        x_n_plus_1 = scipy.integrate.odeint(time_derivative,
+                                            x,
+                                            t,
+                                            args=(voltage, c1, c2, c3))
+        #print ('x_n_plus_1 = ',x_n_plus_1)
+        #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
+        theta, omega = x_n_plus_1[1]
+        #theta= x_n_plus_1[0]
+        #omega = x_n_plus_1[1]
+        if (False):
+            print ("%4d  %8.4f %8.2f          %8.4f          %8.4f    %8.3f    %8.3f     %8.3f      %8.3f" % \
+              (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
+              to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
+        if (theta_start < theta_target):
+            # Angle is increasing through the motion.
+            if (theta > theta_half):
+                break
+        else:
+            # Angle is decreasing through the motion.
+            if (theta < theta_half):
+                break
+
+    #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
+    #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
+    #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
+    return (2.0 * time)
+
 
 def main():
-  global run_count
-  gravity = 9.8 # m/sec^2 Gravity Constant
-  voltage_nominal = 12 # Volts
-  
-  # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
-  motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
-  current_stall = 134 # amps stall current
-  current_no_load = 0.7 # amps no load current
-  torque_stall = 710/1000.0 # N-m Stall Torque
-  speed_no_load_rpm = 18730 # RPM no load speed
-  
-  if ( False ):
-    # Bag motor from https://www.vexrobotics.com/217-3351.html
-    motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
-    current_stall = 53.0 # amps stall current
-    current_no_load = 1.8 # amps no load current
-    torque_stall = 0.4 # N-m Stall Torque
-    speed_no_load_rpm = 13180.0 # RPM no load speed
-  
-  if ( True ):
-    # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
-    motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
-    current_stall = 89.0 # amps stall current
-    current_no_load = 3.0 # amps no load current
-    torque_stall = 1.4 # N-m Stall Torque
-    speed_no_load_rpm = 5840.0 # RPM no load speed
+    global run_count
+    gravity = 9.8  # m/sec^2 Gravity Constant
+    voltage_nominal = 12  # Volts
 
-  # How many motors are we using?
-  num_motors = 1
+    # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
+    motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
+    current_stall = 134  # amps stall current
+    current_no_load = 0.7  # amps no load current
+    torque_stall = 710 / 1000.0  # N-m Stall Torque
+    speed_no_load_rpm = 18730  # RPM no load speed
 
-  # Motor values
-  print ("# Motor: %s" % (motor_name))
-  print ("# Number of motors: %d" % (num_motors))
-  print ("# Stall torque: %.1f n-m" % (torque_stall))
-  print ("# Stall current: %.1f amps" % (current_stall))
-  print ("# No load current: %.1f amps" % (current_no_load))
-  print ("# No load speed: %.0f rpm" % (speed_no_load_rpm))
-  
-  # Constants from motor values
-  resistance_motor = voltage_nominal/current_stall 
-  speed_no_load_rps = speed_no_load_rpm/60.0 # Revolutions per second no load speed
-  speed_no_load = speed_no_load_rps*2.0*pi
-  Kt = num_motors*torque_stall/current_stall # N-m/A torque constant
-  Kv_rpm = speed_no_load_rpm /(voltage_nominal - resistance_motor*current_no_load)  # rpm/V
-  Kv = Kv_rpm*2.0*pi/60.0 # rpm/V
-  
-  # Robot Geometry and physics
-  length_proximal_arm = inches_to_meters*47.34 # m Length of arm connected to the robot base
-  length_distal_arm = inches_to_meters*44.0 # m Length of arm that holds the cube
-  mass_cube = 6.0*lbs_to_kg  # Weight of the cube in Kgrams
-  mass_proximal_arm = 5.5*lbs_to_kg # Weight of proximal arm
-  mass_distal_arm = 3.5*lbs_to_kg # Weight of distal arm
-  mass_distal = mass_cube + mass_distal_arm
-  mass_proximal = mass_proximal_arm + mass_distal
-  radius_to_proximal_arm_cg = 22.0*inches_to_meters # m Length from arm pivot point to arm CG
-  radius_to_distal_arm_cg = 10.0*inches_to_meters # m Length from arm pivot point to arm CG
+    if (False):
+        # Bag motor from https://www.vexrobotics.com/217-3351.html
+        motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
+        current_stall = 53.0  # amps stall current
+        current_no_load = 1.8  # amps no load current
+        torque_stall = 0.4  # N-m Stall Torque
+        speed_no_load_rpm = 13180.0  # RPM no load speed
 
-  radius_to_distal_cg = ( length_distal_arm*mass_cube + radius_to_distal_arm_cg*mass_distal_arm)/mass_distal
-  radius_to_proximal_cg = ( length_proximal_arm*mass_distal + radius_to_proximal_arm_cg*mass_proximal_arm)/mass_proximal
-  J_cube = length_distal_arm*length_distal_arm*mass_cube 
-  # Kg m^2 Moment of inertia of the proximal arm
-  J_proximal_arm = radius_to_proximal_arm_cg*radius_to_proximal_arm_cg*mass_distal_arm 
-  # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
-  J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm*length_proximal_arm*mass_distal 
-  J_distal_arm = radius_to_distal_arm_cg*radius_to_distal_arm_cg*mass_distal_arm # Kg m^2 Moment of inertia of the distal arm
-  J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm # Moment of inertia of the arm with the cube on the end
+    if (True):
+        # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
+        motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
+        current_stall = 89.0  # amps stall current
+        current_no_load = 3.0  # amps no load current
+        torque_stall = 1.4  # N-m Stall Torque
+        speed_no_load_rpm = 5840.0  # RPM no load speed
 
-  error_margine = 1.0
-  voltage = 10.0 # voltage for the motor.  Assuming a loaded robot so not using 12 V.
-  # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
-  # motor_free_speed = Kv*voltage
-  motor_free_speed = speed_no_load
-  
-  print ("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" % (Kt, Kv_rpm, Kv))
-  print ("# %.2f Ohms Resistance of the motor " % (resistance_motor))
-  print ("# %.2f kg Cube weight" % (mass_cube))
-  print ("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
-  print ("# %.2f kg Distal Arm mass" % (mass_distal_arm))
-  print ("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
-  print ("# %.2f m Length from distal arm pivot point to arm CG" % (radius_to_distal_arm_cg))
-  print ("# %.2f m Length from distal arm pivot point to arm and cube cg" % (radius_to_distal_cg))
-  print ("# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point" % (J_cube))
-  print ("# %.2f m Length from proximal arm pivot point to arm CG" % (radius_to_proximal_arm_cg))
-  print ("# %.2f m Length from proximal arm pivot point to arm and cube cg" % (radius_to_proximal_cg))
-  print ("# %.2f m  Proximal arm length" % (length_proximal_arm))
-  print ("# %.2f m  Distal arm length" % (length_distal_arm))
+    # How many motors are we using?
+    num_motors = 1
 
-  print ("# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point" % (J_distal_arm))
-  print ("# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point" % (J_proximal_arm))
-  print ("# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about the proximal arm pivot point" % (J_distal_arm_and_cube_at_end_of_proximal_arm))
-  print ("# %.2f kg-m^2 Moment of inertia of the proximal arm and distal arm and cube about the arm pivot point" % (J))
-  print ("# %d Number of motors" % (num_motors))
-  
-  print ("# %.2f V Motor voltage" % (voltage))
+    # Motor values
+    print("# Motor: %s" % (motor_name))
+    print("# Number of motors: %d" % (num_motors))
+    print("# Stall torque: %.1f n-m" % (torque_stall))
+    print("# Stall current: %.1f amps" % (current_stall))
+    print("# No load current: %.1f amps" % (current_no_load))
+    print("# No load speed: %.0f rpm" % (speed_no_load_rpm))
 
-  print ("\n# Min time is for proximal arm without any forces from distal arm.  Max time puts all distal arm mass at the end of proximal arm.")
+    # Constants from motor values
+    resistance_motor = voltage_nominal / current_stall
+    speed_no_load_rps = speed_no_load_rpm / 60.0  # Revolutions per second no load speed
+    speed_no_load = speed_no_load_rps * 2.0 * pi
+    Kt = num_motors * torque_stall / current_stall  # N-m/A torque constant
+    Kv_rpm = speed_no_load_rpm / (
+        voltage_nominal - resistance_motor * current_no_load)  # rpm/V
+    Kv = Kv_rpm * 2.0 * pi / 60.0  # rpm/V
 
-  for gear_ratio in range(60, 241, 10):
-    c1 = Kt*gear_ratio*gear_ratio/(Kv*resistance_motor*J)
-    c2 = gear_ratio*Kt/(J*resistance_motor)
-    c3 = radius_to_proximal_cg*mass_proximal*gravity/J
-    c1_proximal_only = Kt*gear_ratio*gear_ratio/(Kv*resistance_motor*J_proximal_arm)
-    c2_proximal_only = gear_ratio*Kt/(J_proximal_arm*resistance_motor)
-    c3_proximal_only = radius_to_proximal_arm_cg*mass_proximal_arm*gravity/J_proximal_arm
-  
-    if ( False and run_count < 1 ):
-      print ("# %.8f 1/sec C1 constant" % (c1))
-      print ("# %.2f 1/sec C2 constant" % (c2))
-      print ("# %.2f 1/(V sec^2) C3 constant" % (c3))
-      print ("# %.8f 1/sec C1 proximal only constant" % (c1_proximal_only))
-      print ("# %.2f 1/sec C2 proximal only constant" % (c2_proximal_only))
-      print ("# %.2f 1/(V sec^2) C3 proximal only constant" % (c3_proximal_only))
-      print ("# %.2f RPM Free speed at motor voltage" % (voltage*Kv_rpm))
-  
-    torque_90_degrees = radius_to_proximal_cg*mass_proximal*gravity
-    voltage_90_degrees = resistance_motor*torque_90_degrees/(gear_ratio*Kt)
-    torque_peak = gear_ratio*num_motors*torque_stall
-    torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
-    normal_force = torque_peak/length_proximal_arm
-    normal_force_lbf = newton_to_lbf*normal_force 
-    normal_distal_arm_end_force = torque_peak/(length_proximal_arm + length_distal_arm)
-    normal_distal_arm_end_force_lbf = newton_to_lbf*normal_distal_arm_end_force 
-    time_required = get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed)
-    time_required_proximal_only = get_180_degree_time(c1_proximal_only,c2_proximal_only,c3_proximal_only,voltage,gear_ratio,motor_free_speed)
-    print ("Time for %.1f degrees for gear ratio %3.0f is %.2f (min) %.2f (max) seconds.  90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at proximal end %3.0f N %2.0f lbf distal end %3.0f N %2.0f lbf" % \
-      (to_deg(theta_travel),gear_ratio,time_required_proximal_only,time_required,torque_90_degrees,voltage_90_degrees,
-       torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf,normal_distal_arm_end_force,normal_distal_arm_end_force_lbf))
-  
+    # Robot Geometry and physics
+    length_proximal_arm = inches_to_meters * 47.34  # m Length of arm connected to the robot base
+    length_distal_arm = inches_to_meters * 44.0  # m Length of arm that holds the cube
+    mass_cube = 6.0 * lbs_to_kg  # Weight of the cube in Kgrams
+    mass_proximal_arm = 5.5 * lbs_to_kg  # Weight of proximal arm
+    mass_distal_arm = 3.5 * lbs_to_kg  # Weight of distal arm
+    mass_distal = mass_cube + mass_distal_arm
+    mass_proximal = mass_proximal_arm + mass_distal
+    radius_to_proximal_arm_cg = 22.0 * inches_to_meters  # m Length from arm pivot point to arm CG
+    radius_to_distal_arm_cg = 10.0 * inches_to_meters  # m Length from arm pivot point to arm CG
+
+    radius_to_distal_cg = (
+        length_distal_arm * mass_cube +
+        radius_to_distal_arm_cg * mass_distal_arm) / mass_distal
+    radius_to_proximal_cg = (
+        length_proximal_arm * mass_distal +
+        radius_to_proximal_arm_cg * mass_proximal_arm) / mass_proximal
+    J_cube = length_distal_arm * length_distal_arm * mass_cube
+    # Kg m^2 Moment of inertia of the proximal arm
+    J_proximal_arm = radius_to_proximal_arm_cg * radius_to_proximal_arm_cg * mass_distal_arm
+    # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
+    J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm * length_proximal_arm * mass_distal
+    J_distal_arm = radius_to_distal_arm_cg * radius_to_distal_arm_cg * mass_distal_arm  # Kg m^2 Moment of inertia of the distal arm
+    J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm  # Moment of inertia of the arm with the cube on the end
+
+    error_margine = 1.0
+    voltage = 10.0  # voltage for the motor.  Assuming a loaded robot so not using 12 V.
+    # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
+    # motor_free_speed = Kv*voltage
+    motor_free_speed = speed_no_load
+
+    print("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" %
+          (Kt, Kv_rpm, Kv))
+    print("# %.2f Ohms Resistance of the motor " % (resistance_motor))
+    print("# %.2f kg Cube weight" % (mass_cube))
+    print("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
+    print("# %.2f kg Distal Arm mass" % (mass_distal_arm))
+    print("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
+    print("# %.2f m Length from distal arm pivot point to arm CG" %
+          (radius_to_distal_arm_cg))
+    print("# %.2f m Length from distal arm pivot point to arm and cube cg" %
+          (radius_to_distal_cg))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point"
+        % (J_cube))
+    print("# %.2f m Length from proximal arm pivot point to arm CG" %
+          (radius_to_proximal_arm_cg))
+    print("# %.2f m Length from proximal arm pivot point to arm and cube cg" %
+          (radius_to_proximal_cg))
+    print("# %.2f m  Proximal arm length" % (length_proximal_arm))
+    print("# %.2f m  Distal arm length" % (length_distal_arm))
+
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point"
+        % (J_distal_arm))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point"
+        % (J_proximal_arm))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about the proximal arm pivot point"
+        % (J_distal_arm_and_cube_at_end_of_proximal_arm))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the proximal arm and distal arm and cube about the arm pivot point"
+        % (J))
+    print("# %d Number of motors" % (num_motors))
+
+    print("# %.2f V Motor voltage" % (voltage))
+
+    print(
+        "\n# Min time is for proximal arm without any forces from distal arm.  Max time puts all distal arm mass at the end of proximal arm."
+    )
+
+    for gear_ratio in range(60, 241, 10):
+        c1 = Kt * gear_ratio * gear_ratio / (Kv * resistance_motor * J)
+        c2 = gear_ratio * Kt / (J * resistance_motor)
+        c3 = radius_to_proximal_cg * mass_proximal * gravity / J
+        c1_proximal_only = Kt * gear_ratio * gear_ratio / (
+            Kv * resistance_motor * J_proximal_arm)
+        c2_proximal_only = gear_ratio * Kt / (J_proximal_arm *
+                                              resistance_motor)
+        c3_proximal_only = radius_to_proximal_arm_cg * mass_proximal_arm * gravity / J_proximal_arm
+
+        if (False and run_count < 1):
+            print("# %.8f 1/sec C1 constant" % (c1))
+            print("# %.2f 1/sec C2 constant" % (c2))
+            print("# %.2f 1/(V sec^2) C3 constant" % (c3))
+            print("# %.8f 1/sec C1 proximal only constant" %
+                  (c1_proximal_only))
+            print("# %.2f 1/sec C2 proximal only constant" %
+                  (c2_proximal_only))
+            print("# %.2f 1/(V sec^2) C3 proximal only constant" %
+                  (c3_proximal_only))
+            print("# %.2f RPM Free speed at motor voltage" %
+                  (voltage * Kv_rpm))
+
+        torque_90_degrees = radius_to_proximal_cg * mass_proximal * gravity
+        voltage_90_degrees = resistance_motor * torque_90_degrees / (
+            gear_ratio * Kt)
+        torque_peak = gear_ratio * num_motors * torque_stall
+        torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
+        normal_force = torque_peak / length_proximal_arm
+        normal_force_lbf = newton_to_lbf * normal_force
+        normal_distal_arm_end_force = torque_peak / (length_proximal_arm +
+                                                     length_distal_arm)
+        normal_distal_arm_end_force_lbf = newton_to_lbf * normal_distal_arm_end_force
+        time_required = get_180_degree_time(c1, c2, c3, voltage, gear_ratio,
+                                            motor_free_speed)
+        time_required_proximal_only = get_180_degree_time(
+            c1_proximal_only, c2_proximal_only, c3_proximal_only, voltage,
+            gear_ratio, motor_free_speed)
+        print ("Time for %.1f degrees for gear ratio %3.0f is %.2f (min) %.2f (max) seconds.  90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at proximal end %3.0f N %2.0f lbf distal end %3.0f N %2.0f lbf" % \
+          (to_deg(theta_travel),gear_ratio,time_required_proximal_only,time_required,torque_90_degrees,voltage_90_degrees,
+           torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf,normal_distal_arm_end_force,normal_distal_arm_end_force_lbf))
+
+
 if __name__ == '__main__':
-   main()
+    main()
diff --git a/y2018/control_loops/python/arm_trajectory.py b/y2018/control_loops/python/arm_trajectory.py
index bcfd9f0..9db7717 100755
--- a/y2018/control_loops/python/arm_trajectory.py
+++ b/y2018/control_loops/python/arm_trajectory.py
@@ -20,6 +20,7 @@
 
 
 class Dynamics(object):
+
     def __init__(self, dt):
         self.dt = dt
 
@@ -61,29 +62,30 @@
             [[self.G1 * self.Kt / self.R, 0.0],
              [0.0, self.G2 * self.kNumDistalMotors * self.Kt / self.R]])
         self.K4 = numpy.matrix(
-            [[self.G1 * self.G1 * self.Kt / (self.Kv * self.R), 0.0], [
-                0.0, self.G2 * self.G2 * self.Kt * self.kNumDistalMotors /
-                (self.Kv * self.R)
-            ]])
+            [[self.G1 * self.G1 * self.Kt / (self.Kv * self.R), 0.0],
+             [
+                 0.0, self.G2 * self.G2 * self.Kt * self.kNumDistalMotors /
+                 (self.Kv * self.R)
+             ]])
 
         # These constants are for the Extended Kalman Filter
         # Q is the covariance of the X values.  Use the square of the standard
         # deviation of the error accumulated each time step.
-        self.Q_x_covariance = numpy.matrix([[0.001**2,0.0,0.0,0.0,0.0,0.0],
-                                       [0.0,0.001**2,0.0,0.0,0.0,0.0],
-                                       [0.0,0.0,0.001**2,0.0,0.0,0.0],
-                                       [0.0,0.0,0.0,0.001**2,0.0,0.0],
-                                       [0.0,0.0,0.0,0.0,10.0**2,0.0],
-                                       [0.0,0.0,0.0,0.0,0.0,10.0**2]])
+        self.Q_x_covariance = numpy.matrix(
+            [[0.001**2, 0.0, 0.0, 0.0, 0.0, 0.0],
+             [0.0, 0.001**2, 0.0, 0.0, 0.0, 0.0],
+             [0.0, 0.0, 0.001**2, 0.0, 0.0, 0.0],
+             [0.0, 0.0, 0.0, 0.001**2, 0.0, 0.0],
+             [0.0, 0.0, 0.0, 0.0, 10.0**2, 0.0],
+             [0.0, 0.0, 0.0, 0.0, 0.0, 10.0**2]])
         # R is the covariance of the Z values.  Increase the responsiveness to
         # changes by reducing coresponding term in the R matrix.
-        self.R_y_covariance = numpy.matrix([[0.01**2, 0.0],[0.0, 0.01**2]])
+        self.R_y_covariance = numpy.matrix([[0.01**2, 0.0], [0.0, 0.01**2]])
         # H is the jacobian of the h(x) measurement prediction function
-        self.H_h_jacobian = numpy.matrix([[1.0,0.0,0.0,0.0,0.0,0.0],
-                                          [0.0,0.0,1.0,0.0,0.0,0.0]])
+        self.H_h_jacobian = numpy.matrix([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+                                          [0.0, 0.0, 1.0, 0.0, 0.0, 0.0]])
         self.Identity_matrix = numpy.matrix(numpy.identity(6))
 
-
     def discrete_dynamics_ekf_predict(self, X_hat, P_covariance_estimate, U,
                                       sim_dt):
         """Updates the Extended Kalman Filter state for one timestep.
@@ -108,8 +110,8 @@
         # Predict step
         #   Compute the state trasition matrix using the Jacobian of state
         #   estimate
-        F_k = numerical_jacobian_x(self.unbounded_discrete_dynamics_ekf,
-          X_hat, U)
+        F_k = numerical_jacobian_x(self.unbounded_discrete_dynamics_ekf, X_hat,
+                                   U)
         #   State estimate
         X_hat = self.unbounded_discrete_dynamics_ekf(X_hat, U, sim_dt)
         #   Covariance estimate
@@ -134,7 +136,7 @@
         # Update step
         #   Measurement residual error of proximal and distal
         #   joint angles.
-        Y_hat = Y_reading - numpy.matrix([[X_hat[0,0]],[X_hat[2,0]]])
+        Y_hat = Y_reading - numpy.matrix([[X_hat[0, 0]], [X_hat[2, 0]]])
         #   Residual covariance
         S = self.H_h_jacobian * P_covariance_estimate * self.H_h_jacobian.T + \
           self.R_y_covariance
@@ -144,8 +146,8 @@
         #   Updated state estimate
         X_hat = X_hat + Kalman_gain * Y_hat
         #   Updated covariance estimate
-        P_covariance_estimate = (self.Identity_matrix -
-          Kalman_gain * self.H_h_jacobian) * P_covariance_estimate
+        P_covariance_estimate = (self.Identity_matrix - Kalman_gain *
+                                 self.H_h_jacobian) * P_covariance_estimate
         return X_hat, P_covariance_estimate
 
     def NormilizedMatriciesForState(self, X):
@@ -158,8 +160,8 @@
         c = numpy.cos(X[0, 0] - X[2, 0])
 
         # K1 * d^2 theta/dt^2 + K2 * d theta/dt = torque
-        K1 = numpy.matrix(
-            [[self.alpha, c * self.beta], [c * self.beta, self.gamma]])
+        K1 = numpy.matrix([[self.alpha, c * self.beta],
+                           [c * self.beta, self.gamma]])
 
         K2 = numpy.matrix([[0.0, s * self.beta], [-s * self.beta, 0.0]])
 
@@ -183,8 +185,8 @@
         """
         K1, K2, K3, K4 = self.MatriciesForState(X)
 
-        return numpy.linalg.inv(K3) * (
-            K1 * alpha_t + K2 * omega_t + K4 * omega_t)
+        return numpy.linalg.inv(K3) * (K1 * alpha_t + K2 * omega_t +
+                                       K4 * omega_t)
 
     def ff_u_distance(self, trajectory, distance):
         """Computes the feed forward U at the distance on the trajectory.
@@ -252,15 +254,15 @@
                              [accel[1, 0]], [0.0], [0.0]])
 
     def unbounded_discrete_dynamics(self, X, U, dt=None):
-        return RungeKutta(lambda startingX: self.dynamics(startingX, U), X,
-                          dt or self.dt)
+        return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt
+                          or self.dt)
 
     def unbounded_discrete_dynamics_ekf(self, X, U, dt=None):
         return RungeKutta(lambda startingX: self.dynamics_ekf(startingX, U), X,
                           dt or self.dt)
 
     def discrete_dynamics(self, X, U, dt=None):
-        assert((numpy.abs(U) <= (12.0 + 1e-6)).all())
+        assert ((numpy.abs(U) <= (12.0 + 1e-6)).all())
         return self.unbounded_discrete_dynamics(X, U, dt)
 
 
@@ -324,8 +326,8 @@
     q_vel = 4.0
     Q = numpy.matrix(
         numpy.diag([
-            1.0 / (q_pos**2.0), 1.0 / (q_vel**2.0), 1.0 / (q_pos**2.0), 1.0 / (
-                q_vel**2.0)
+            1.0 / (q_pos**2.0), 1.0 / (q_vel**2.0), 1.0 / (q_pos**2.0),
+            1.0 / (q_vel**2.0)
         ]))
 
     R = numpy.matrix(numpy.diag([1.0 / (12.0**2.0), 1.0 / (12.0**2.0)]))
@@ -335,19 +337,21 @@
 
     return controls.dlqr(final_A, final_B, Q, R)
 
+
 def get_encoder_values(X):
-  """Returns simulated encoder readings.
+    """Returns simulated encoder readings.
 
   This method returns the encoder readings.  For now simply use values from X
   with some noise added in to make the simulation more interesting.
   """
-  introduced_random_measurement_noise = 0.005
-  introduced_random_measurement_noise = 0.05
-  theta1_measured = X[0,0] + introduced_random_measurement_noise * \
-    2.0 * ( numpy.random.random() - 0.5 )
-  theta2_measured = X[2,0] + introduced_random_measurement_noise * \
-    2.0 * ( numpy.random.random() - 0.5 )
-  return numpy.matrix([[theta1_measured ],[theta2_measured]])
+    introduced_random_measurement_noise = 0.005
+    introduced_random_measurement_noise = 0.05
+    theta1_measured = X[0,0] + introduced_random_measurement_noise * \
+      2.0 * ( numpy.random.random() - 0.5 )
+    theta2_measured = X[2,0] + introduced_random_measurement_noise * \
+      2.0 * ( numpy.random.random() - 0.5 )
+    return numpy.matrix([[theta1_measured], [theta2_measured]])
+
 
 class Trajectory:
     """This class represents a trajectory in theta space."""
@@ -358,8 +362,12 @@
             numpy.matrix([[numpy.pi / 2.0 - x[0]], [numpy.pi / 2.0 - x[1]]])
             for x in self.path_points
         ]
-        self._omegas = [numpy.matrix([[-x[2]], [-x[3]]]) for x in self.path_points]
-        self._alphas = [numpy.matrix([[-x[4]], [-x[5]]]) for x in self.path_points]
+        self._omegas = [
+            numpy.matrix([[-x[2]], [-x[3]]]) for x in self.path_points
+        ]
+        self._alphas = [
+            numpy.matrix([[-x[4]], [-x[5]]]) for x in self.path_points
+        ]
         self.path_step_size = path_step_size
         self.point_distances = [0.0]
         last_point = self._thetas[0]
@@ -389,8 +397,9 @@
             return points[0]
         elif distance >= self._length:
             return points[-1]
-        after_index = numpy.searchsorted(
-            self.point_distances, distance, side='right')
+        after_index = numpy.searchsorted(self.point_distances,
+                                         distance,
+                                         side='right')
         before_index = after_index - 1
         return (distance - self.point_distances[before_index]) / (
             self.point_distances[after_index] -
@@ -420,15 +429,15 @@
             alpha = self.alpha(distance)
             X = numpy.matrix([[theta[0, 0]], [0.0], [theta[1, 0]], [0.0]])
             K1, K2, K3, K4 = dynamics.NormilizedMatriciesForState(X)
-            omega_square = numpy.matrix(
-                [[omega[0, 0], 0.0], [0.0, omega[1, 0]]])
+            omega_square = numpy.matrix([[omega[0, 0], 0.0],
+                                         [0.0, omega[1, 0]]])
             # Here, we can say that
             #   d^2/dt^2 theta = d^2/dd^2 theta(d) * (d d/dt)^2
             # Normalize so that the max accel is 1, and take magnitudes. This
             # gives us the max velocity we can be at each point due to
             # curvature.
-            vk1 = numpy.linalg.inv(K3) * (
-                K1 * alpha + K2 * omega_square * omega)
+            vk1 = numpy.linalg.inv(K3) * (K1 * alpha +
+                                          K2 * omega_square * omega)
             vk2 = numpy.linalg.inv(K3) * K4 * omega
             ddots = []
             for c in [-vmax, vmax]:
@@ -470,8 +479,8 @@
 
         voltage_accel_list = []
         for c in [-vmax, vmax]:
-            for a, b in [(k_constant[0, 0], k_scalar[0, 0]), (k_constant[1, 0],
-                                                              k_scalar[1, 0])]:
+            for a, b in [(k_constant[0, 0], k_scalar[0, 0]),
+                         (k_constant[1, 0], k_scalar[1, 0])]:
                 # This time, we are doing the other pass.  So, find all
                 # the decelerations (and flip them) to find the prior
                 # velocity.
@@ -483,14 +492,16 @@
                 filtered_voltage_accel_list.append(a)
 
         goal_acceleration = numpy.sqrt(
-            max(0.0, 1.0 -
+            max(
+                0.0, 1.0 -
                 (numpy.linalg.norm(alpha_unitizer * alpha) * velocity *
                  velocity)**2.0)) / numpy.linalg.norm(alpha_unitizer * omega)
         if filtered_voltage_accel_list:
             # TODO(austin): The max of the list seems right, but I'm
             # not seeing many lists with a size > 1, so it's hard to
             # tell.  Max is conservative, for sure.
-            goal_acceleration = min(-max(filtered_voltage_accel_list), goal_acceleration)
+            goal_acceleration = min(-max(filtered_voltage_accel_list),
+                                    goal_acceleration)
         return goal_acceleration
 
     def back_trajectory_pass(self, previous_pass, dynamics, alpha_unitizer,
@@ -516,8 +527,8 @@
 
                 integration_step_size = self.path_step_size / float(num_steps)
                 int_d += integration_step_size
-                int_vel = numpy.sqrt(2.0 * int_accel_t * integration_step_size
-                                     + int_vel * int_vel)
+                int_vel = numpy.sqrt(2.0 * int_accel_t *
+                                     integration_step_size + int_vel * int_vel)
             max_dvelocity_back_pass[index] = min(
                 int_vel, max_dvelocity_back_pass[index])
         return max_dvelocity_back_pass
@@ -539,17 +550,18 @@
         omega_square = numpy.matrix([[omega[0, 0], 0.0], [0.0, omega[1, 0]]])
 
         k_constant = numpy.linalg.inv(K3) * (
-            (K1 * alpha + K2 * omega_square * omega
-             ) * goal_velocity * goal_velocity + K4 * omega * goal_velocity)
+            (K1 * alpha + K2 * omega_square * omega) * goal_velocity *
+            goal_velocity + K4 * omega * goal_velocity)
         k_scalar = numpy.linalg.inv(K3) * K1 * omega
         voltage_accel_list = []
         for c in [-vmax, vmax]:
-            for a, b in [(k_constant[0, 0], k_scalar[0, 0]), (k_constant[1, 0],
-                                                              k_scalar[1, 0])]:
+            for a, b in [(k_constant[0, 0], k_scalar[0, 0]),
+                         (k_constant[1, 0], k_scalar[1, 0])]:
                 voltage_accel_list.append((c - a) / b)
 
         goal_acceleration = numpy.sqrt(
-            max(0.0, 1.0 -
+            max(
+                0.0, 1.0 -
                 (numpy.linalg.norm(alpha_unitizer * alpha) * goal_velocity *
                  goal_velocity)**2.0)) / numpy.linalg.norm(
                      alpha_unitizer * omega)
@@ -564,8 +576,8 @@
             # TODO(austin): The max of the list seems right, but I'm not
             # seeing many lists with a size > 1, so it's hard to tell.
             # Min is conservative, for sure.
-            goal_acceleration = min(
-                min(filtered_voltage_accel_list), goal_acceleration)
+            goal_acceleration = min(min(filtered_voltage_accel_list),
+                                    goal_acceleration)
 
         return goal_acceleration
 
@@ -592,8 +604,8 @@
 
                 integration_step_size = self.path_step_size / float(num_steps)
                 int_d += integration_step_size
-                int_vel = numpy.sqrt(2.0 * int_accel_t * integration_step_size
-                                      + int_vel * int_vel)
+                int_vel = numpy.sqrt(2.0 * int_accel_t *
+                                     integration_step_size + int_vel * int_vel)
 
             max_dvelocity_forward_pass[index] = min(
                 int_vel, max_dvelocity_forward_pass[index])
@@ -623,11 +635,12 @@
 
     def interpolate_velocity(self, d, d0, d1, v0, v1):
         if v0 + v1 > 0:
-            return numpy.sqrt(v0 * v0 +
-                              (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0))
+            return numpy.sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) /
+                              (d1 - d0))
         else:
-            return -numpy.sqrt(v0 * v0 +
-                               (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0))
+            return -numpy.sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) /
+                               (d1 - d0))
+
     def get_dvelocity(self, d):
         """Computes the path distance velocity of the plan as a function of the distance."""
         after_index = numpy.argmax(self.distance_array > d)
@@ -662,8 +675,8 @@
         if d > self._length:
             return numpy.matrix(numpy.zeros((2, 1)))
         return self.alpha(d) * (
-            (velocity or self.get_dvelocity(d))**2.0) + self.omega(d) * (
-                acceleration or self.get_dacceleration(d))
+            (velocity or self.get_dvelocity(d))**
+            2.0) + self.omega(d) * (acceleration or self.get_dacceleration(d))
 
     def R(self, d, velocity=None):
         theta_t = self.theta(d)
@@ -682,21 +695,17 @@
     # TODO(austin): use computed forward dynamics velocity here.
     theta_t = trajectory.theta(saturation_goal_distance)
     saturation_goal_velocity = trajectory.interpolate_velocity(
-        saturation_goal_distance, last_goal_distance,
-        goal_distance, last_goal_velocity, goal_velocity)
+        saturation_goal_distance, last_goal_distance, goal_distance,
+        last_goal_velocity, goal_velocity)
     saturation_goal_acceleration = trajectory.interpolate_acceleration(
-        last_goal_distance, goal_distance, last_goal_velocity,
-        goal_velocity)
-    omega_t = trajectory.omega_t(
-        saturation_goal_distance,
-        velocity=saturation_goal_velocity)
-    alpha_t = trajectory.alpha_t(
-        saturation_goal_distance,
-        velocity=saturation_goal_velocity,
-        acceleration=saturation_goal_acceleration)
-    R = trajectory.R(
-        saturation_goal_distance,
-        velocity=saturation_goal_velocity)
+        last_goal_distance, goal_distance, last_goal_velocity, goal_velocity)
+    omega_t = trajectory.omega_t(saturation_goal_distance,
+                                 velocity=saturation_goal_velocity)
+    alpha_t = trajectory.alpha_t(saturation_goal_distance,
+                                 velocity=saturation_goal_velocity,
+                                 acceleration=saturation_goal_acceleration)
+    R = trajectory.R(saturation_goal_distance,
+                     velocity=saturation_goal_velocity)
     U_ff = numpy.clip(dynamics.ff_u(R, omega_t, alpha_t), -12.0, 12.0)
     return U_ff + K * (
         R - X), saturation_goal_velocity, saturation_goal_acceleration
@@ -767,12 +776,15 @@
     alpha0_max = 40.0
     alpha1_max = 60.0
 
-    alpha_unitizer = numpy.matrix(
-        [[1.0 / alpha0_max, 0.0], [0.0, 1.0 / alpha1_max]])
+    alpha_unitizer = numpy.matrix([[1.0 / alpha0_max, 0.0],
+                                   [0.0, 1.0 / alpha1_max]])
 
     # Compute the trajectory taking into account our velocity, acceleration
     # and voltage constraints.
-    trajectory.compute_trajectory(dynamics, alpha_unitizer, distance_array, vmax=vmax)
+    trajectory.compute_trajectory(dynamics,
+                                  alpha_unitizer,
+                                  distance_array,
+                                  vmax=vmax)
 
     print 'Computed trajectory'
 
@@ -820,8 +832,8 @@
     theta_t = trajectory.theta(goal_distance)
     X = numpy.matrix([[theta_t[0, 0]], [0.0], [theta_t[1, 0]], [0.0]])
     # X_hat is for the Extended Kalman Filter state estimate
-    X_hat = numpy.matrix([[theta_t[0, 0]], [0.0], [theta_t[1, 0]],
-      [0.0], [0.0], [0.0]])
+    X_hat = numpy.matrix([[theta_t[0, 0]], [0.0], [theta_t[1, 0]], [0.0],
+                          [0.0], [0.0]])
     # P is the Covariance Estimate for the Etended Kalman Filter
     P_covariance_estimate = dynamics.Q_x_covariance.copy()
 
@@ -856,10 +868,9 @@
         t_array.append(t)
         theta_t = trajectory.theta(goal_distance)
         omega_t = trajectory.omega_t(goal_distance, velocity=goal_velocity)
-        alpha_t = trajectory.alpha_t(
-            goal_distance,
-            velocity=goal_velocity,
-            acceleration=goal_acceleration)
+        alpha_t = trajectory.alpha_t(goal_distance,
+                                     velocity=goal_velocity,
+                                     acceleration=goal_acceleration)
 
         theta0_goal_t_array.append(theta_t[0, 0])
         theta1_goal_t_array.append(theta_t[1, 0])
@@ -887,7 +898,7 @@
         # available.  For now, simulate the sensor reading by using the X
         # position and adding some noise to it.
         X_hat, P_covariance_estimate = dynamics.discrete_dynamics_ekf_update(
-          X_hat, P_covariance_estimate, sim_dt, get_encoder_values(X))
+            X_hat, P_covariance_estimate, sim_dt, get_encoder_values(X))
 
         R = trajectory.R(goal_distance, velocity=goal_velocity)
         U_ff = numpy.clip(dynamics.ff_u(R, omega_t, alpha_t), -12.0, 12.0)
@@ -927,8 +938,9 @@
                     fraction_along_path += step_size
             print "Fraction", fraction_along_path, "at", goal_distance, "rad,", t, "sec", goal_velocity
 
-            goal_distance = ((goal_distance - last_goal_distance) *
-                             fraction_along_path + last_goal_distance)
+            goal_distance = (
+                (goal_distance - last_goal_distance) * fraction_along_path +
+                last_goal_distance)
             goal_velocity = saturation_goal_velocity
             goal_acceleration = saturation_goal_acceleration
 
@@ -965,8 +977,7 @@
         # Push Extended Kalman filter state forwards.
         # Predict step - call for each time step
         X_hat, P_covariance_estimate = dynamics.discrete_dynamics_ekf_predict(
-          X_hat, P_covariance_estimate, U, sim_dt)
-
+            X_hat, P_covariance_estimate, U, sim_dt)
 
         if abs(goal_distance - trajectory.length()) < 1e-2:
             # If we go backwards along the path near the goal, snap us to the
@@ -999,12 +1010,15 @@
 
     pylab.figure()
     pylab.title("Path Velocity Plan")
-    pylab.plot(
-        distance_array, trajectory.max_dvelocity_unfiltered, label="pass0")
-    pylab.plot(
-        distance_array, trajectory.max_dvelocity_back_pass, label="passb")
-    pylab.plot(
-        distance_array, trajectory.max_dvelocity_forward_pass, label="passf")
+    pylab.plot(distance_array,
+               trajectory.max_dvelocity_unfiltered,
+               label="pass0")
+    pylab.plot(distance_array,
+               trajectory.max_dvelocity_back_pass,
+               label="passb")
+    pylab.plot(distance_array,
+               trajectory.max_dvelocity_forward_pass,
+               label="passf")
     pylab.legend(loc='center')
     pylab.legend()
 
@@ -1062,11 +1076,14 @@
 
     pylab.figure()
     pylab.title("Disturbance Force from Extended Kalman Filter State Values")
-    pylab.plot(t_array, torque_disturbance_0_hat_array, label="torque_disturbance_0_hat")
-    pylab.plot(t_array, torque_disturbance_1_hat_array, label="torque_disturbance_1_hat")
+    pylab.plot(t_array,
+               torque_disturbance_0_hat_array,
+               label="torque_disturbance_0_hat")
+    pylab.plot(t_array,
+               torque_disturbance_1_hat_array,
+               label="torque_disturbance_1_hat")
     pylab.legend()
 
-
     pylab.show()
 
 
diff --git a/y2018/control_loops/python/drivetrain.py b/y2018/control_loops/python/drivetrain.py
index 675930a..0e5ed43 100644
--- a/y2018/control_loops/python/drivetrain.py
+++ b/y2018/control_loops/python/drivetrain.py
@@ -47,5 +47,6 @@
         # Write the generated constants out to a file.
         drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2018', kDrivetrain)
 
+
 if __name__ == '__main__':
     sys.exit(main(sys.argv))
diff --git a/y2018/control_loops/python/extended_lqr.py b/y2018/control_loops/python/extended_lqr.py
index 4eecee9..9a4705b 100755
--- a/y2018/control_loops/python/extended_lqr.py
+++ b/y2018/control_loops/python/extended_lqr.py
@@ -11,16 +11,17 @@
 
 
 class ArmDynamics(object):
-  def __init__(self, dt):
-    self.dt = dt
 
-    self.l1 = 1.0
-    self.l2 = 0.8
-    self.num_states = 4
-    self.num_inputs = 2
+    def __init__(self, dt):
+        self.dt = dt
 
-  def dynamics(self, X, U):
-    """Calculates the dynamics for a double jointed arm.
+        self.l1 = 1.0
+        self.l2 = 0.8
+        self.num_states = 4
+        self.num_inputs = 2
+
+    def dynamics(self, X, U):
+        """Calculates the dynamics for a double jointed arm.
 
     Args:
       X, numpy.matrix(4, 1), The state.  [theta1, omega1, theta2, omega2]
@@ -29,48 +30,53 @@
     Returns:
       numpy.matrix(4, 1), The derivative of the dynamics.
     """
-    return numpy.matrix([[X[1, 0]],
-                         [U[0, 0]],
-                         [X[3, 0]],
-                         [U[1, 0]]])
+        return numpy.matrix([[X[1, 0]], [U[0, 0]], [X[3, 0]], [U[1, 0]]])
 
-  def discrete_dynamics(self, X, U):
-    return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
+    def discrete_dynamics(self, X, U):
+        return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
 
-  def inverse_discrete_dynamics(self, X, U):
-    return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X, dt)
+    def inverse_discrete_dynamics(self, X, U):
+        return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X,
+                          dt)
+
 
 # Simple implementation for a quadratic cost function.
 class ArmCostFunction:
-  def __init__(self, dt, dynamics):
-    self.num_states = 4
-    self.num_inputs = 2
-    self.dt = dt
-    self.dynamics = dynamics
 
-    q_pos = 0.5
-    q_vel = 1.65
-    self.Q = numpy.matrix(numpy.diag([
-        1.0 / (q_pos ** 2.0), 1.0 / (q_vel ** 2.0),
-        1.0 / (q_pos ** 2.0), 1.0 / (q_vel ** 2.0)]))
+    def __init__(self, dt, dynamics):
+        self.num_states = 4
+        self.num_inputs = 2
+        self.dt = dt
+        self.dynamics = dynamics
 
-    self.R = numpy.matrix(numpy.diag([1.0 / (12.0 ** 2.0),
-                                      1.0 / (12.0 ** 2.0)]))
+        q_pos = 0.5
+        q_vel = 1.65
+        self.Q = numpy.matrix(
+            numpy.diag([
+                1.0 / (q_pos**2.0), 1.0 / (q_vel**2.0), 1.0 / (q_pos**2.0),
+                1.0 / (q_vel**2.0)
+            ]))
 
-    final_A = numerical_jacobian_x(self.dynamics.discrete_dynamics,
-                                   numpy.matrix(numpy.zeros((4, 1))),
-                                   numpy.matrix(numpy.zeros((2, 1))))
-    final_B = numerical_jacobian_u(self.dynamics.discrete_dynamics,
-                                   numpy.matrix(numpy.zeros((4, 1))),
-                                   numpy.matrix(numpy.zeros((2, 1))))
-    print 'Final A', final_A
-    print 'Final B', final_B
-    K, self.S = controls.dlqr(
-        final_A, final_B, self.Q, self.R, optimal_cost_function=True) 
-    print 'Final eig:', numpy.linalg.eig(final_A - final_B * K)
+        self.R = numpy.matrix(
+            numpy.diag([1.0 / (12.0**2.0), 1.0 / (12.0**2.0)]))
 
-  def final_cost(self, X, U):
-    """Computes the final cost of being at X
+        final_A = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+                                       numpy.matrix(numpy.zeros((4, 1))),
+                                       numpy.matrix(numpy.zeros((2, 1))))
+        final_B = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+                                       numpy.matrix(numpy.zeros((4, 1))),
+                                       numpy.matrix(numpy.zeros((2, 1))))
+        print 'Final A', final_A
+        print 'Final B', final_B
+        K, self.S = controls.dlqr(final_A,
+                                  final_B,
+                                  self.Q,
+                                  self.R,
+                                  optimal_cost_function=True)
+        print 'Final eig:', numpy.linalg.eig(final_A - final_B * K)
+
+    def final_cost(self, X, U):
+        """Computes the final cost of being at X
 
     Args:
       X: numpy.matrix(self.num_states, 1)
@@ -79,10 +85,10 @@
     Returns:
       numpy.matrix(1, 1), The quadratic cost of being at X
     """
-    return 0.5 * X.T * self.S * X
+        return 0.5 * X.T * self.S * X
 
-  def cost(self, X, U):
-    """Computes the incremental cost given a position and U.
+    def cost(self, X, U):
+        """Computes the incremental cost given a position and U.
 
     Args:
       X: numpy.matrix(self.num_states, 1)
@@ -91,10 +97,10 @@
     Returns:
       numpy.matrix(1, 1), The quadratic cost of evaluating U.
     """
-    return U.T * self.R * U + X.T * self.Q * X
+        return U.T * self.R * U + X.T * self.Q * X
 
-  def estimate_Q_final(self, X_hat):
-    """Returns the quadraticized final Q around X_hat.
+    def estimate_Q_final(self, X_hat):
+        """Returns the quadraticized final Q around X_hat.
 
     This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
 
@@ -104,13 +110,13 @@
     Result:
       numpy.matrix(self.num_states, self.num_states)
     """
-    zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
-    print 'S', self.S
-    print 'Q_final', numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
-    return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+        zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+        print 'S', self.S
+        print 'Q_final', numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+        return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
 
-  def estimate_partial_cost_partial_x_final(self, X_hat):
-    """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
+    def estimate_partial_cost_partial_x_final(self, X_hat):
+        """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
 
     Args:
       X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
@@ -118,24 +124,26 @@
     Result:
       numpy.matrix(self.num_states, 1)
     """
-    return numerical_jacobian_x(self.final_cost, X_hat,
-                                numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
+        return numerical_jacobian_x(
+            self.final_cost, X_hat,
+            numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
 
-  def estimate_q_final(self, X_hat):
-    """Returns q evaluated at X_hat for the final cost function."""
-    return self.estimate_partial_cost_partial_x_final(X_hat) - self.estimate_Q_final(X_hat) * X_hat
-
+    def estimate_q_final(self, X_hat):
+        """Returns q evaluated at X_hat for the final cost function."""
+        return self.estimate_partial_cost_partial_x_final(
+            X_hat) - self.estimate_Q_final(X_hat) * X_hat
 
 
 class SkidSteerDynamics(object):
-  def __init__(self, dt):
-    self.width = 0.2
-    self.dt = dt
-    self.num_states = 3
-    self.num_inputs = 2
 
-  def dynamics(self, X, U):
-    """Calculates the dynamics for a 2 wheeled robot.
+    def __init__(self, dt):
+        self.width = 0.2
+        self.dt = dt
+        self.num_states = 3
+        self.num_inputs = 2
+
+    def dynamics(self, X, U):
+        """Calculates the dynamics for a 2 wheeled robot.
 
     Args:
       X, numpy.matrix(3, 1), The state.  [x, y, theta]
@@ -144,34 +152,34 @@
     Returns:
       numpy.matrix(3, 1), The derivative of the dynamics.
     """
-    #return numpy.matrix([[X[1, 0]],
-    #                     [X[2, 0]],
-    #                     [U[0, 0]]])
-    return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
-                         [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
-                         [(U[1, 0] - U[0, 0]) / self.width]])
+        #return numpy.matrix([[X[1, 0]],
+        #                     [X[2, 0]],
+        #                     [U[0, 0]]])
+        return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
+                             [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
+                             [(U[1, 0] - U[0, 0]) / self.width]])
 
-  def discrete_dynamics(self, X, U):
-    return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
+    def discrete_dynamics(self, X, U):
+        return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
 
-  def inverse_discrete_dynamics(self, X, U):
-    return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X, dt)
+    def inverse_discrete_dynamics(self, X, U):
+        return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X,
+                          dt)
 
 
 # Simple implementation for a quadratic cost function.
 class CostFunction:
-  def __init__(self, dt):
-    self.num_states = 3
-    self.num_inputs = 2
-    self.dt = dt
-    self.Q = numpy.matrix([[0.1, 0, 0],
-                           [0, 0.6, 0],
-                           [0, 0, 0.1]]) / self.dt / self.dt
-    self.R = numpy.matrix([[0.40, 0],
-                           [0, 0.40]]) / self.dt / self.dt
 
-  def final_cost(self, X, U):
-    """Computes the final cost of being at X
+    def __init__(self, dt):
+        self.num_states = 3
+        self.num_inputs = 2
+        self.dt = dt
+        self.Q = numpy.matrix([[0.1, 0, 0], [0, 0.6, 0], [0, 0, 0.1]
+                               ]) / self.dt / self.dt
+        self.R = numpy.matrix([[0.40, 0], [0, 0.40]]) / self.dt / self.dt
+
+    def final_cost(self, X, U):
+        """Computes the final cost of being at X
 
     Args:
       X: numpy.matrix(self.num_states, 1)
@@ -180,10 +188,10 @@
     Returns:
       numpy.matrix(1, 1), The quadratic cost of being at X
     """
-    return X.T * self.Q * X * 1000
+        return X.T * self.Q * X * 1000
 
-  def cost(self, X, U):
-    """Computes the incremental cost given a position and U.
+    def cost(self, X, U):
+        """Computes the incremental cost given a position and U.
 
     Args:
       X: numpy.matrix(self.num_states, 1)
@@ -192,10 +200,10 @@
     Returns:
       numpy.matrix(1, 1), The quadratic cost of evaluating U.
     """
-    return U.T * self.R * U + X.T * self.Q * X
+        return U.T * self.R * U + X.T * self.Q * X
 
-  def estimate_Q_final(self, X_hat):
-    """Returns the quadraticized final Q around X_hat.
+    def estimate_Q_final(self, X_hat):
+        """Returns the quadraticized final Q around X_hat.
 
     This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
 
@@ -205,11 +213,11 @@
     Result:
       numpy.matrix(self.num_states, self.num_states)
     """
-    zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
-    return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+        zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+        return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
 
-  def estimate_partial_cost_partial_x_final(self, X_hat):
-    """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
+    def estimate_partial_cost_partial_x_final(self, X_hat):
+        """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
 
     Args:
       X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
@@ -217,23 +225,27 @@
     Result:
       numpy.matrix(self.num_states, 1)
     """
-    return numerical_jacobian_x(self.final_cost, X_hat, numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
+        return numerical_jacobian_x(
+            self.final_cost, X_hat,
+            numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
 
-  def estimate_q_final(self, X_hat):
-    """Returns q evaluated at X_hat for the final cost function."""
-    return self.estimate_partial_cost_partial_x_final(X_hat) - self.estimate_Q_final(X_hat) * X_hat
+    def estimate_q_final(self, X_hat):
+        """Returns q evaluated at X_hat for the final cost function."""
+        return self.estimate_partial_cost_partial_x_final(
+            X_hat) - self.estimate_Q_final(X_hat) * X_hat
 
 
 def RungeKutta(f, x, dt):
-  """4th order RungeKutta integration of F starting at X."""
-  a = f(x)
-  b = f(x + dt / 2.0 * a)
-  c = f(x + dt / 2.0 * b)
-  d = f(x + dt * c)
-  return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+    """4th order RungeKutta integration of F starting at X."""
+    a = f(x)
+    b = f(x + dt / 2.0 * a)
+    c = f(x + dt / 2.0 * b)
+    d = f(x + dt * c)
+    return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
 
 def numerical_jacobian_x(fn, X, U, epsilon=1e-4):
-  """Numerically estimates the jacobian around X, U in X.
+    """Numerically estimates the jacobian around X, U in X.
 
   Args:
     fn: A function of X, U.
@@ -246,20 +258,21 @@
     numpy.matrix(num_states, num_states), The jacobian of fn with X as the
       variable.
   """
-  num_states = X.shape[0]
-  nominal = fn(X, U)
-  answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
-  # It's more expensive, but +- epsilon will be more reliable
-  for i in range(0, num_states):
-    dX_plus = X.copy()
-    dX_plus[i] += epsilon
-    dX_minus = X.copy()
-    dX_minus[i] -= epsilon
-    answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
-  return answer
+    num_states = X.shape[0]
+    nominal = fn(X, U)
+    answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
+    # It's more expensive, but +- epsilon will be more reliable
+    for i in range(0, num_states):
+        dX_plus = X.copy()
+        dX_plus[i] += epsilon
+        dX_minus = X.copy()
+        dX_minus[i] -= epsilon
+        answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
+    return answer
+
 
 def numerical_jacobian_u(fn, X, U, epsilon=1e-4):
-  """Numerically estimates the jacobian around X, U in U.
+    """Numerically estimates the jacobian around X, U in U.
 
   Args:
     fn: A function of X, U.
@@ -272,355 +285,426 @@
     numpy.matrix(num_states, num_inputs), The jacobian of fn with U as the
       variable.
   """
-  num_states = X.shape[0]
-  num_inputs = U.shape[0]
-  nominal = fn(X, U)
-  answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
-  for i in range(0, num_inputs):
-    dU_plus = U.copy()
-    dU_plus[i] += epsilon
-    dU_minus = U.copy()
-    dU_minus[i] -= epsilon
-    answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
-  return answer
+    num_states = X.shape[0]
+    num_inputs = U.shape[0]
+    nominal = fn(X, U)
+    answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
+    for i in range(0, num_inputs):
+        dU_plus = U.copy()
+        dU_plus[i] += epsilon
+        dU_minus = U.copy()
+        dU_minus[i] -= epsilon
+        answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
+    return answer
+
 
 def numerical_jacobian_x_x(fn, X, U):
-  return numerical_jacobian_x(
-      lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+    return numerical_jacobian_x(
+        lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+        X, U)
+
 
 def numerical_jacobian_x_u(fn, X, U):
-  return numerical_jacobian_x(
-      lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+    return numerical_jacobian_x(
+        lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+        X, U)
+
 
 def numerical_jacobian_u_x(fn, X, U):
-  return numerical_jacobian_u(
-      lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+    return numerical_jacobian_u(
+        lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+        X, U)
+
 
 def numerical_jacobian_u_u(fn, X, U):
-  return numerical_jacobian_u(
-      lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+    return numerical_jacobian_u(
+        lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+        X, U)
 
 
 class ELQR(object):
-  def __init__(self, dynamics, cost):
-    self.dynamics = dynamics
-    self.cost = cost
-  
-  def Solve(self, x_hat_initial, horizon, iterations):
-    l = horizon
-    num_states = self.dynamics.num_states
-    num_inputs = self.dynamics.num_inputs
-    self.S_bar_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
-    self.s_bar_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
-    self.s_scalar_bar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
 
-    self.L_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
-    self.l_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
-    self.L_bar_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
-    self.l_bar_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
+    def __init__(self, dynamics, cost):
+        self.dynamics = dynamics
+        self.cost = cost
 
-    self.S_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
-    self.s_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
-    self.s_scalar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
+    def Solve(self, x_hat_initial, horizon, iterations):
+        l = horizon
+        num_states = self.dynamics.num_states
+        num_inputs = self.dynamics.num_inputs
+        self.S_bar_t = [
+            numpy.matrix(numpy.zeros((num_states, num_states)))
+            for _ in range(l + 1)
+        ]
+        self.s_bar_t = [
+            numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+        ]
+        self.s_scalar_bar_t = [
+            numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)
+        ]
 
-    self.last_x_hat_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+        self.L_t = [
+            numpy.matrix(numpy.zeros((num_inputs, num_states)))
+            for _ in range(l + 1)
+        ]
+        self.l_t = [
+            numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)
+        ]
+        self.L_bar_t = [
+            numpy.matrix(numpy.zeros((num_inputs, num_states)))
+            for _ in range(l + 1)
+        ]
+        self.l_bar_t = [
+            numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)
+        ]
 
-    # Iterate the solver
-    for a in range(iterations):
-      x_hat = x_hat_initial
-      u_t = self.L_t[0] * x_hat + self.l_t[0]
-      self.S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
-      self.s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
-      self.s_scalar_bar_t[0] = numpy.matrix([[0]])
+        self.S_t = [
+            numpy.matrix(numpy.zeros((num_states, num_states)))
+            for _ in range(l + 1)
+        ]
+        self.s_t = [
+            numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+        ]
+        self.s_scalar_t = [
+            numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)
+        ]
 
-      self.last_x_hat_t[0] = x_hat_initial
+        self.last_x_hat_t = [
+            numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+        ]
 
-      Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_initial, u_t)
-      P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_initial, u_t)
-      R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_initial, u_t)
+        # Iterate the solver
+        for a in range(iterations):
+            x_hat = x_hat_initial
+            u_t = self.L_t[0] * x_hat + self.l_t[0]
+            self.S_bar_t[0] = numpy.matrix(
+                numpy.zeros((num_states, num_states)))
+            self.s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
+            self.s_scalar_bar_t[0] = numpy.matrix([[0]])
 
-      q_t = numerical_jacobian_x(self.cost.cost, x_hat_initial, u_t).T \
-          - Q_t * x_hat_initial - P_t.T * u_t
-      r_t = numerical_jacobian_u(self.cost.cost, x_hat_initial, u_t).T \
-          - P_t * x_hat_initial - R_t * u_t
+            self.last_x_hat_t[0] = x_hat_initial
 
-      q_scalar_t = self.cost.cost(x_hat_initial, u_t) \
-          - 0.5 * (x_hat_initial.T * (Q_t * x_hat_initial + P_t.T * u_t) \
-          + u_t.T * (P_t * x_hat_initial + R_t * u_t)) \
-          - x_hat_initial.T * q_t - u_t.T * r_t
+            Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_initial, u_t)
+            P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_initial, u_t)
+            R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_initial, u_t)
 
-      start_A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics, x_hat_initial, u_t)
-      start_B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics, x_hat_initial, u_t)
-      x_hat_next = self.dynamics.discrete_dynamics(x_hat_initial, u_t)
-      start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
+            q_t = numerical_jacobian_x(self.cost.cost, x_hat_initial, u_t).T \
+                - Q_t * x_hat_initial - P_t.T * u_t
+            r_t = numerical_jacobian_u(self.cost.cost, x_hat_initial, u_t).T \
+                - P_t * x_hat_initial - R_t * u_t
 
-      B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
-      B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
-      B_svd_sigma[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = \
-          numpy.diag(B_svd_sigma_diag)
+            q_scalar_t = self.cost.cost(x_hat_initial, u_t) \
+                - 0.5 * (x_hat_initial.T * (Q_t * x_hat_initial + P_t.T * u_t) \
+                + u_t.T * (P_t * x_hat_initial + R_t * u_t)) \
+                - x_hat_initial.T * q_t - u_t.T * r_t
 
-      B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
-      B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0],
-                      0:B_svd_sigma_diag.shape[0]] = \
-                          numpy.linalg.inv(numpy.diag(B_svd_sigma_diag))
-      B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
+            start_A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+                                             x_hat_initial, u_t)
+            start_B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+                                             x_hat_initial, u_t)
+            x_hat_next = self.dynamics.discrete_dynamics(x_hat_initial, u_t)
+            start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
 
-      self.L_bar_t[1] = B_svd_inv
-      self.l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
+            B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
+            B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
+            B_svd_sigma[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = \
+                numpy.diag(B_svd_sigma_diag)
 
-      self.S_bar_t[1] = self.L_bar_t[1].T * R_t * self.L_bar_t[1]
+            B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
+            B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0],
+                            0:B_svd_sigma_diag.shape[0]] = \
+                                numpy.linalg.inv(numpy.diag(B_svd_sigma_diag))
+            B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
 
-      TotalS_1 = start_B_t.T * self.S_t[1] * start_B_t + R_t
-      Totals_1 = start_B_t.T * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
-          + start_B_t.T * self.s_t[1] + P_t * x_hat_initial + r_t
-      Totals_scalar_1 = 0.5 * (start_c_t.T + x_hat_initial.T * start_A_t.T) * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
-          + self.s_scalar_t[1] + x_hat_initial.T * q_t + q_scalar_t \
-          + 0.5 * x_hat_initial.T * Q_t * x_hat_initial \
-          + (start_c_t.T + x_hat_initial.T * start_A_t.T) * self.s_t[1]
+            self.L_bar_t[1] = B_svd_inv
+            self.l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial +
+                                            start_c_t)
 
-      optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
-      optimal_x_1 = start_A_t * x_hat_initial \
-          + start_B_t * optimal_u_1 + start_c_t
+            self.S_bar_t[1] = self.L_bar_t[1].T * R_t * self.L_bar_t[1]
 
-      # TODO(austin): Disable this if we are controlable.  It should not be needed then.
-      S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = \
-          numpy.linalg.eigh(self.S_bar_t[1])
-      S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
-      S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
-      for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
-        if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
-          S_bar_1_eigh_eigenvalues_stiff[i] = max(S_bar_1_eigh_eigenvalues_stiff) * 1.0
+            TotalS_1 = start_B_t.T * self.S_t[1] * start_B_t + R_t
+            Totals_1 = start_B_t.T * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
+                + start_B_t.T * self.s_t[1] + P_t * x_hat_initial + r_t
+            Totals_scalar_1 = 0.5 * (start_c_t.T + x_hat_initial.T * start_A_t.T) * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
+                + self.s_scalar_t[1] + x_hat_initial.T * q_t + q_scalar_t \
+                + 0.5 * x_hat_initial.T * Q_t * x_hat_initial \
+                + (start_c_t.T + x_hat_initial.T * start_A_t.T) * self.s_t[1]
 
-      S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
+            optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
+            optimal_x_1 = start_A_t * x_hat_initial \
+                + start_B_t * optimal_u_1 + start_c_t
 
-      print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
-      print 'Min x_hat', optimal_x_1
-      self.s_bar_t[1] = -self.s_t[1] - (S_bar_stiff + self.S_t[1]) * optimal_x_1
-      self.s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 \
-          - optimal_x_1.T * (S_bar_stiff + self.S_t[1]) * optimal_x_1) \
-          + optimal_u_1.T * Totals_1 \
-          - optimal_x_1.T * (self.s_bar_t[1] + self.s_t[1]) \
-          - self.s_scalar_t[1] + Totals_scalar_1
+            # TODO(austin): Disable this if we are controlable.  It should not be needed then.
+            S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = \
+                numpy.linalg.eigh(self.S_bar_t[1])
+            S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
+            S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
+            for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
+                if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
+                    S_bar_1_eigh_eigenvalues_stiff[i] = max(
+                        S_bar_1_eigh_eigenvalues_stiff) * 1.0
 
-      print 'optimal_u_1', optimal_u_1
-      print 'TotalS_1', TotalS_1
-      print 'Totals_1', Totals_1
-      print 'Totals_scalar_1', Totals_scalar_1
-      print 'overall cost 1', 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1) \
-                               + optimal_u_1.T * Totals_1 + Totals_scalar_1
-      print 'overall cost 0', 0.5 * (x_hat_initial.T * self.S_t[0] * x_hat_initial) \
-                              + x_hat_initial.T * self.s_t[0] + self.s_scalar_t[0]
+            S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(
+                numpy.diag(S_bar_1_eigh_eigenvalues_stiff)
+            ) * S_bar_1_eigh_eigenvectors.T
 
-      print 't forward 0'
-      print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
-      print 'x_hat[%2d]: %s' % (0, x_hat.T)
-      print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
-      print 'u[%2d]: %s' % (0, u_t.T)
-      print ('L[ 0]: %s' % (self.L_t[0],)).replace('\n', '\n        ')
-      print ('l[ 0]: %s' % (self.l_t[0],)).replace('\n', '\n        ')
+            print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
+            print 'Min x_hat', optimal_x_1
+            self.s_bar_t[1] = -self.s_t[1] - (S_bar_stiff +
+                                              self.S_t[1]) * optimal_x_1
+            self.s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 \
+                - optimal_x_1.T * (S_bar_stiff + self.S_t[1]) * optimal_x_1) \
+                + optimal_u_1.T * Totals_1 \
+                - optimal_x_1.T * (self.s_bar_t[1] + self.s_t[1]) \
+                - self.s_scalar_t[1] + Totals_scalar_1
 
-      print ('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n         ')
-      print ('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n         ')
-      print ('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n         ')
+            print 'optimal_u_1', optimal_u_1
+            print 'TotalS_1', TotalS_1
+            print 'Totals_1', Totals_1
+            print 'Totals_scalar_1', Totals_scalar_1
+            print 'overall cost 1', 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1) \
+                                     + optimal_u_1.T * Totals_1 + Totals_scalar_1
+            print 'overall cost 0', 0.5 * (x_hat_initial.T * self.S_t[0] * x_hat_initial) \
+                                    + x_hat_initial.T * self.s_t[0] + self.s_scalar_t[0]
 
-      # TODO(austin): optimal_x_1 is x_hat
-      x_hat = -numpy.linalg.solve((self.S_t[1] + S_bar_stiff),
-                                  (self.s_t[1] + self.s_bar_t[1]))
-      print 'new xhat', x_hat
+            print 't forward 0'
+            print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
+            print 'x_hat[%2d]: %s' % (0, x_hat.T)
+            print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
+            print 'u[%2d]: %s' % (0, u_t.T)
+            print('L[ 0]: %s' % (self.L_t[0], )).replace('\n', '\n        ')
+            print('l[ 0]: %s' % (self.l_t[0], )).replace('\n', '\n        ')
 
-      self.S_bar_t[1] = S_bar_stiff
+            print('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n         ')
+            print('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n         ')
+            print('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n         ')
 
-      self.last_x_hat_t[1] = x_hat
+            # TODO(austin): optimal_x_1 is x_hat
+            x_hat = -numpy.linalg.solve((self.S_t[1] + S_bar_stiff),
+                                        (self.s_t[1] + self.s_bar_t[1]))
+            print 'new xhat', x_hat
 
-      for t in range(1, l):
-        print 't forward', t
-        u_t = self.L_t[t] * x_hat + self.l_t[t]
+            self.S_bar_t[1] = S_bar_stiff
 
-        x_hat_next = self.dynamics.discrete_dynamics(x_hat, u_t)
-        A_bar_t = numerical_jacobian_x(self.dynamics.inverse_discrete_dynamics,
-                                       x_hat_next, u_t)
-        B_bar_t = numerical_jacobian_u(self.dynamics.inverse_discrete_dynamics,
-                                       x_hat_next, u_t)
-        c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
+            self.last_x_hat_t[1] = x_hat
 
-        print 'x_hat[%2d]: %s' % (t, x_hat.T)
-        print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
-        print ('L[%2d]: %s' % (t, self.L_t[t],)).replace('\n', '\n        ')
-        print ('l[%2d]: %s' % (t, self.l_t[t],)).replace('\n', '\n        ')
-        print 'u[%2d]: %s' % (t, u_t.T)
+            for t in range(1, l):
+                print 't forward', t
+                u_t = self.L_t[t] * x_hat + self.l_t[t]
 
-        print ('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace('\n', '\n             ')
-        print ('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace('\n', '\n             ')
-        print ('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace('\n', '\n             ')
+                x_hat_next = self.dynamics.discrete_dynamics(x_hat, u_t)
+                A_bar_t = numerical_jacobian_x(
+                    self.dynamics.inverse_discrete_dynamics, x_hat_next, u_t)
+                B_bar_t = numerical_jacobian_u(
+                    self.dynamics.inverse_discrete_dynamics, x_hat_next, u_t)
+                c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
 
-        Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat, u_t)
-        P_t = numerical_jacobian_x_u(self.cost.cost, x_hat, u_t)
-        R_t = numerical_jacobian_u_u(self.cost.cost, x_hat, u_t)
+                print 'x_hat[%2d]: %s' % (t, x_hat.T)
+                print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
+                print('L[%2d]: %s' % (
+                    t,
+                    self.L_t[t],
+                )).replace('\n', '\n        ')
+                print('l[%2d]: %s' % (
+                    t,
+                    self.l_t[t],
+                )).replace('\n', '\n        ')
+                print 'u[%2d]: %s' % (t, u_t.T)
 
-        q_t = numerical_jacobian_x(self.cost.cost, x_hat, u_t).T \
-            - Q_t * x_hat - P_t.T * u_t
-        r_t = numerical_jacobian_u(self.cost.cost, x_hat, u_t).T \
-            - P_t * x_hat - R_t * u_t
+                print('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace(
+                    '\n', '\n             ')
+                print('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace(
+                    '\n', '\n             ')
+                print('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace(
+                    '\n', '\n             ')
 
-        q_scalar_t = self.cost.cost(x_hat, u_t) \
-            - 0.5 * (x_hat.T * (Q_t * x_hat + P_t.T * u_t) \
-            + u_t.T * (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
+                Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat, u_t)
+                P_t = numerical_jacobian_x_u(self.cost.cost, x_hat, u_t)
+                R_t = numerical_jacobian_u_u(self.cost.cost, x_hat, u_t)
 
-        C_bar_t = B_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
-        D_bar_t = A_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t
-        E_bar_t = B_bar_t.T * (self.S_bar_t[t] + Q_t) * B_bar_t + R_t \
-            + P_t * B_bar_t + B_bar_t.T * P_t.T
-        d_bar_t = A_bar_t.T * (self.s_bar_t[t] + q_t) \
-            + A_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
-        e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * self.s_bar_t[t] \
-            + B_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
+                q_t = numerical_jacobian_x(self.cost.cost, x_hat, u_t).T \
+                    - Q_t * x_hat - P_t.T * u_t
+                r_t = numerical_jacobian_u(self.cost.cost, x_hat, u_t).T \
+                    - P_t * x_hat - R_t * u_t
 
-        self.L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
-        self.l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
+                q_scalar_t = self.cost.cost(x_hat, u_t) \
+                    - 0.5 * (x_hat.T * (Q_t * x_hat + P_t.T * u_t) \
+                    + u_t.T * (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
 
-        self.S_bar_t[t + 1] = D_bar_t + C_bar_t.T * self.L_bar_t[t + 1]
-        self.s_bar_t[t + 1] = d_bar_t + C_bar_t.T * self.l_bar_t[t + 1]
-        self.s_scalar_bar_t[t + 1] = \
-            -0.5 * e_bar_t.T * numpy.linalg.inv(E_bar_t) * e_bar_t \
-            + 0.5 * c_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t \
-            + c_bar_t.T * self.s_bar_t[t] + c_bar_t.T * q_t \
-            + self.s_scalar_bar_t[t] + q_scalar_t
+                C_bar_t = B_bar_t.T * (self.S_bar_t[t] +
+                                       Q_t) * A_bar_t + P_t * A_bar_t
+                D_bar_t = A_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t
+                E_bar_t = B_bar_t.T * (self.S_bar_t[t] + Q_t) * B_bar_t + R_t \
+                    + P_t * B_bar_t + B_bar_t.T * P_t.T
+                d_bar_t = A_bar_t.T * (self.s_bar_t[t] + q_t) \
+                    + A_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
+                e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * self.s_bar_t[t] \
+                    + B_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
 
-        x_hat = -numpy.linalg.solve((self.S_t[t + 1] + self.S_bar_t[t + 1]),
-                                    (self.s_t[t + 1] + self.s_bar_t[t + 1]))
-        self.S_t[l] = self.cost.estimate_Q_final(x_hat)
-      self.s_t[l] = self.cost.estimate_q_final(x_hat)
-      x_hat = -numpy.linalg.inv(self.S_t[l] + self.S_bar_t[l]) \
-          * (self.s_t[l] + self.s_bar_t[l])
+                self.L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
+                self.l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
 
-      for t in reversed(range(l)):
-        print 't backward', t
-        # TODO(austin): I don't think we can use L_t like this here.
-        # I think we are off by 1 somewhere...
-        u_t = self.L_bar_t[t + 1] * x_hat + self.l_bar_t[t + 1]
+                self.S_bar_t[t + 1] = D_bar_t + C_bar_t.T * self.L_bar_t[t + 1]
+                self.s_bar_t[t + 1] = d_bar_t + C_bar_t.T * self.l_bar_t[t + 1]
+                self.s_scalar_bar_t[t + 1] = \
+                    -0.5 * e_bar_t.T * numpy.linalg.inv(E_bar_t) * e_bar_t \
+                    + 0.5 * c_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t \
+                    + c_bar_t.T * self.s_bar_t[t] + c_bar_t.T * q_t \
+                    + self.s_scalar_bar_t[t] + q_scalar_t
 
-        x_hat_prev = self.dynamics.inverse_discrete_dynamics(x_hat, u_t)
-        print 'x_hat[%2d]: %s' % (t, x_hat.T)
-        print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
-        print ('L_bar[%2d]: %s' % (t + 1, self.L_bar_t[t + 1])).replace('\n', '\n            ')
-        print ('l_bar[%2d]: %s' % (t + 1, self.l_bar_t[t + 1])).replace('\n', '\n            ')
-        print 'u[%2d]: %s' % (t, u_t.T)
-        # Now compute the linearized A, B, and C
-        # Start by doing it numerically, and then optimize.
-        A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics, x_hat_prev, u_t)
-        B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics, x_hat_prev, u_t)
-        c_t = x_hat - A_t * x_hat_prev - B_t * u_t
+                x_hat = -numpy.linalg.solve(
+                    (self.S_t[t + 1] + self.S_bar_t[t + 1]),
+                    (self.s_t[t + 1] + self.s_bar_t[t + 1]))
+                self.S_t[l] = self.cost.estimate_Q_final(x_hat)
+            self.s_t[l] = self.cost.estimate_q_final(x_hat)
+            x_hat = -numpy.linalg.inv(self.S_t[l] + self.S_bar_t[l]) \
+                * (self.s_t[l] + self.s_bar_t[l])
 
-        print ('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n         ')
-        print ('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n         ')
-        print ('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n         ')
+            for t in reversed(range(l)):
+                print 't backward', t
+                # TODO(austin): I don't think we can use L_t like this here.
+                # I think we are off by 1 somewhere...
+                u_t = self.L_bar_t[t + 1] * x_hat + self.l_bar_t[t + 1]
 
-        Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_prev, u_t)
-        P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_prev, u_t)
-        P_T_t = numerical_jacobian_u_x(self.cost.cost, x_hat_prev, u_t)
-        R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_prev, u_t)
+                x_hat_prev = self.dynamics.inverse_discrete_dynamics(
+                    x_hat, u_t)
+                print 'x_hat[%2d]: %s' % (t, x_hat.T)
+                print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
+                print('L_bar[%2d]: %s' % (t + 1, self.L_bar_t[t + 1])).replace(
+                    '\n', '\n            ')
+                print('l_bar[%2d]: %s' % (t + 1, self.l_bar_t[t + 1])).replace(
+                    '\n', '\n            ')
+                print 'u[%2d]: %s' % (t, u_t.T)
+                # Now compute the linearized A, B, and C
+                # Start by doing it numerically, and then optimize.
+                A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+                                           x_hat_prev, u_t)
+                B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+                                           x_hat_prev, u_t)
+                c_t = x_hat - A_t * x_hat_prev - B_t * u_t
 
-        q_t = numerical_jacobian_x(self.cost.cost, x_hat_prev, u_t).T \
-            - Q_t * x_hat_prev - P_T_t * u_t
-        r_t = numerical_jacobian_u(self.cost.cost, x_hat_prev, u_t).T \
-            - P_t * x_hat_prev - R_t * u_t
+                print('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n         ')
+                print('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n         ')
+                print('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n         ')
 
-        q_scalar_t = self.cost.cost(x_hat_prev, u_t) \
-            - 0.5 * (x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) \
-            + u_t.T * (P_t * x_hat_prev + R_t * u_t)) \
-            - x_hat_prev.T * q_t - u_t.T * r_t
+                Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_prev, u_t)
+                P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_prev, u_t)
+                P_T_t = numerical_jacobian_u_x(self.cost.cost, x_hat_prev, u_t)
+                R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_prev, u_t)
 
-        C_t = P_t + B_t.T * self.S_t[t + 1] * A_t
-        D_t = Q_t + A_t.T * self.S_t[t + 1] * A_t
-        E_t = R_t + B_t.T * self.S_t[t + 1] * B_t
-        d_t = q_t + A_t.T * self.s_t[t + 1] + A_t.T * self.S_t[t + 1] * c_t
-        e_t = r_t + B_t.T * self.s_t[t + 1] + B_t.T * self.S_t[t + 1] * c_t
-        self.L_t[t] = -numpy.linalg.inv(E_t) * C_t
-        self.l_t[t] = -numpy.linalg.inv(E_t) * e_t
-        self.s_t[t] = d_t + C_t.T * self.l_t[t]
-        self.S_t[t] = D_t + C_t.T * self.L_t[t]
-        self.s_scalar_t[t] = q_scalar_t \
-            - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t \
-            + 0.5 * c_t.T * self.S_t[t + 1] * c_t \
-            + c_t.T * self.s_t[t + 1] \
-            + self.s_scalar_t[t + 1]
+                q_t = numerical_jacobian_x(self.cost.cost, x_hat_prev, u_t).T \
+                    - Q_t * x_hat_prev - P_T_t * u_t
+                r_t = numerical_jacobian_u(self.cost.cost, x_hat_prev, u_t).T \
+                    - P_t * x_hat_prev - R_t * u_t
 
-        x_hat = -numpy.linalg.solve((self.S_t[t] + self.S_bar_t[t]),
-                                    (self.s_t[t] + self.s_bar_t[t]))
-        if t == 0:
-          self.last_x_hat_t[t] = x_hat_initial
-        else:
-          self.last_x_hat_t[t] = x_hat
+                q_scalar_t = self.cost.cost(x_hat_prev, u_t) \
+                    - 0.5 * (x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) \
+                    + u_t.T * (P_t * x_hat_prev + R_t * u_t)) \
+                    - x_hat_prev.T * q_t - u_t.T * r_t
 
-      x_hat_t = [x_hat_initial]
+                C_t = P_t + B_t.T * self.S_t[t + 1] * A_t
+                D_t = Q_t + A_t.T * self.S_t[t + 1] * A_t
+                E_t = R_t + B_t.T * self.S_t[t + 1] * B_t
+                d_t = q_t + A_t.T * self.s_t[t + 1] + A_t.T * self.S_t[t +
+                                                                       1] * c_t
+                e_t = r_t + B_t.T * self.s_t[t + 1] + B_t.T * self.S_t[t +
+                                                                       1] * c_t
+                self.L_t[t] = -numpy.linalg.inv(E_t) * C_t
+                self.l_t[t] = -numpy.linalg.inv(E_t) * e_t
+                self.s_t[t] = d_t + C_t.T * self.l_t[t]
+                self.S_t[t] = D_t + C_t.T * self.L_t[t]
+                self.s_scalar_t[t] = q_scalar_t \
+                    - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t \
+                    + 0.5 * c_t.T * self.S_t[t + 1] * c_t \
+                    + c_t.T * self.s_t[t + 1] \
+                    + self.s_scalar_t[t + 1]
 
-      pylab.figure('states %d' % a)
-      pylab.ion()
-      for dim in range(num_states):
-        pylab.plot(numpy.arange(len(self.last_x_hat_t)),
-                   [x_hat_loop[dim, 0] for x_hat_loop in self.last_x_hat_t],
-                   marker='o', label='Xhat[%d]' % dim)
-      pylab.legend()
-      pylab.draw()
+                x_hat = -numpy.linalg.solve((self.S_t[t] + self.S_bar_t[t]),
+                                            (self.s_t[t] + self.s_bar_t[t]))
+                if t == 0:
+                    self.last_x_hat_t[t] = x_hat_initial
+                else:
+                    self.last_x_hat_t[t] = x_hat
 
-      pylab.figure('xy %d' % a)
-      pylab.ion()
-      pylab.plot([x_hat_loop[0, 0] for x_hat_loop in self.last_x_hat_t],
-                 [x_hat_loop[1, 0] for x_hat_loop in self.last_x_hat_t],
-                 marker='o', label='trajectory')
-      pylab.legend()
-      pylab.draw()
+            x_hat_t = [x_hat_initial]
 
-    final_u_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
-    cost_to_go = []
-    cost_to_come = []
-    cost = []
-    for t in range(l):
-      cost_to_go.append(
-          (0.5 * self.last_x_hat_t[t].T * self.S_t[t] * self.last_x_hat_t[t] \
-          + self.last_x_hat_t[t].T * self.s_t[t] + self.s_scalar_t[t])[0, 0])
-      cost_to_come.append(
-          (0.5 * self.last_x_hat_t[t].T * self.S_bar_t[t] * self.last_x_hat_t[t] \
-           + self.last_x_hat_t[t].T * self.s_bar_t[t] + self.s_scalar_bar_t[t])[0, 0])
-      cost.append(cost_to_go[-1] + cost_to_come[-1])
-      final_u_t[t] = self.L_t[t] * self.last_x_hat_t[t] + self.l_t[t]
+            pylab.figure('states %d' % a)
+            pylab.ion()
+            for dim in range(num_states):
+                pylab.plot(
+                    numpy.arange(len(self.last_x_hat_t)),
+                    [x_hat_loop[dim, 0] for x_hat_loop in self.last_x_hat_t],
+                    marker='o',
+                    label='Xhat[%d]' % dim)
+            pylab.legend()
+            pylab.draw()
 
-    for t in range(l):
-      A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
-                                 self.last_x_hat_t[t], final_u_t[t])
-      B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
-                                 self.last_x_hat_t[t], final_u_t[t])
-      c_t = self.dynamics.discrete_dynamics(self.last_x_hat_t[t], final_u_t[t]) \
-          - A_t * self.last_x_hat_t[t] - B_t * final_u_t[t]
-      print("Infeasability at", t, "is",
-            ((A_t * self.last_x_hat_t[t] + B_t * final_u_t[t] + c_t) \
-            - self.last_x_hat_t[t + 1]).T)
+            pylab.figure('xy %d' % a)
+            pylab.ion()
+            pylab.plot([x_hat_loop[0, 0] for x_hat_loop in self.last_x_hat_t],
+                       [x_hat_loop[1, 0] for x_hat_loop in self.last_x_hat_t],
+                       marker='o',
+                       label='trajectory')
+            pylab.legend()
+            pylab.draw()
 
-    pylab.figure('u')
-    samples = numpy.arange(len(final_u_t))
-    for i in range(num_inputs):
-      pylab.plot(samples, [u[i, 0] for u in final_u_t],
-                 label='u[%d]' % i, marker='o')
-      pylab.legend()
+        final_u_t = [
+            numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)
+        ]
+        cost_to_go = []
+        cost_to_come = []
+        cost = []
+        for t in range(l):
+            cost_to_go.append(
+                (0.5 * self.last_x_hat_t[t].T * self.S_t[t] * self.last_x_hat_t[t] \
+                + self.last_x_hat_t[t].T * self.s_t[t] + self.s_scalar_t[t])[0, 0])
+            cost_to_come.append(
+                (0.5 * self.last_x_hat_t[t].T * self.S_bar_t[t] * self.last_x_hat_t[t] \
+                 + self.last_x_hat_t[t].T * self.s_bar_t[t] + self.s_scalar_bar_t[t])[0, 0])
+            cost.append(cost_to_go[-1] + cost_to_come[-1])
+            final_u_t[t] = self.L_t[t] * self.last_x_hat_t[t] + self.l_t[t]
 
-    pylab.figure('cost')
-    cost_samples = numpy.arange(len(cost))
-    pylab.plot(cost_samples, cost_to_go, label='cost to go', marker='o')
-    pylab.plot(cost_samples, cost_to_come, label='cost to come', marker='o')
-    pylab.plot(cost_samples, cost, label='cost', marker='o')
-    pylab.legend()
+        for t in range(l):
+            A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+                                       self.last_x_hat_t[t], final_u_t[t])
+            B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+                                       self.last_x_hat_t[t], final_u_t[t])
+            c_t = self.dynamics.discrete_dynamics(self.last_x_hat_t[t], final_u_t[t]) \
+                - A_t * self.last_x_hat_t[t] - B_t * final_u_t[t]
+            print("Infeasability at", t, "is",
+                  ((A_t * self.last_x_hat_t[t] + B_t * final_u_t[t] + c_t) \
+                  - self.last_x_hat_t[t + 1]).T)
 
-    pylab.ioff()
-    pylab.show()
+        pylab.figure('u')
+        samples = numpy.arange(len(final_u_t))
+        for i in range(num_inputs):
+            pylab.plot(samples, [u[i, 0] for u in final_u_t],
+                       label='u[%d]' % i,
+                       marker='o')
+            pylab.legend()
+
+        pylab.figure('cost')
+        cost_samples = numpy.arange(len(cost))
+        pylab.plot(cost_samples, cost_to_go, label='cost to go', marker='o')
+        pylab.plot(cost_samples,
+                   cost_to_come,
+                   label='cost to come',
+                   marker='o')
+        pylab.plot(cost_samples, cost, label='cost', marker='o')
+        pylab.legend()
+
+        pylab.ioff()
+        pylab.show()
+
 
 if __name__ == '__main__':
-  dt = 0.05
-  #arm_dynamics = ArmDynamics(dt=dt)
-  #elqr = ELQR(arm_dynamics, ArmCostFunction(dt=dt, dynamics=arm_dynamics))
-  #x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0], [0.0]])
-  #elqr.Solve(x_hat_initial, 100, 3)
+    dt = 0.05
+    #arm_dynamics = ArmDynamics(dt=dt)
+    #elqr = ELQR(arm_dynamics, ArmCostFunction(dt=dt, dynamics=arm_dynamics))
+    #x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0], [0.0]])
+    #elqr.Solve(x_hat_initial, 100, 3)
 
-  elqr = ELQR(SkidSteerDynamics(dt=dt), CostFunction(dt=dt))
-  x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
-  elqr.Solve(x_hat_initial, 100, 15)
-  sys.exit(1)
+    elqr = ELQR(SkidSteerDynamics(dt=dt), CostFunction(dt=dt))
+    x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
+    elqr.Solve(x_hat_initial, 100, 15)
+    sys.exit(1)
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index 22bd1d2..e7f8ce1 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -23,16 +23,16 @@
         alpha_unitizer = "(::Eigen::Matrix<double, 2, 2>() << %f, %f, %f, %f).finished()" % (
             segment.alpha_unitizer[0, 0], segment.alpha_unitizer[0, 1],
             segment.alpha_unitizer[1, 0], segment.alpha_unitizer[1, 1])
-    cc_file.append( "  trajectories->emplace_back(%s," % (vmax))
-    cc_file.append( "                             %s," % (alpha_unitizer))
+    cc_file.append("  trajectories->emplace_back(%s," % (vmax))
+    cc_file.append("                             %s," % (alpha_unitizer))
     if reverse:
         cc_file.append(
             "                             Trajectory(Path::Reversed(%s()), 0.005));"
             % (path_function_name(str(name))))
     else:
         cc_file.append(
-            "                             Trajectory(%s(), 0.005));"
-            % (path_function_name(str(name))))
+            "                             Trajectory(%s(), 0.005));" %
+            (path_function_name(str(name))))
 
     start_index = None
     end_index = None
@@ -45,15 +45,15 @@
     if reverse:
         start_index, end_index = end_index, start_index
 
-    cc_file.append("  edges.push_back(SearchGraph::Edge{%s(), %s()," %
-                   (index_function_name(start_index),
-                    index_function_name(end_index)))
     cc_file.append(
-        "                     (trajectories->back().trajectory.path().length() + 0.2)});")
+        "  edges.push_back(SearchGraph::Edge{%s(), %s()," %
+        (index_function_name(start_index), index_function_name(end_index)))
+    cc_file.append(
+        "                     (trajectories->back().trajectory.path().length() + 0.2)});"
+    )
 
     # TODO(austin): Allow different vmaxes for different paths.
-    cc_file.append(
-        "  trajectories->back().trajectory.OptimizeTrajectory(")
+    cc_file.append("  trajectories->back().trajectory.OptimizeTrajectory(")
     cc_file.append("      trajectories->back().alpha_unitizer,")
     cc_file.append("      trajectories->back().vmax);")
     cc_file.append("")
@@ -116,15 +116,16 @@
         h_file.append("")
         h_file.append("constexpr uint32_t %s() { return %d; }" %
                       (index_function_name(point[1]), index))
-        h_file.append(
-            "inline ::Eigen::Matrix<double, 2, 1> %sPoint() {" % point[1])
+        h_file.append("inline ::Eigen::Matrix<double, 2, 1> %sPoint() {" %
+                      point[1])
         h_file.append(
             "  return (::Eigen::Matrix<double, 2, 1>() << %f, %f).finished();"
             % (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
         h_file.append("}")
 
     front_points = [
-        index_function_name(point[1]) + "()" for point in graph_generate.front_points
+        index_function_name(point[1]) + "()"
+        for point in graph_generate.front_points
     ]
     h_file.append("")
     h_file.append("constexpr ::std::array<uint32_t, %d> FrontPoints() {" %
@@ -134,7 +135,8 @@
     h_file.append("}")
 
     back_points = [
-        index_function_name(point[1]) + "()" for point in graph_generate.back_points
+        index_function_name(point[1]) + "()"
+        for point in graph_generate.back_points
     ]
     h_file.append("")
     h_file.append("constexpr ::std::array<uint32_t, %d> BackPoints() {" %
@@ -149,10 +151,10 @@
     for name, segment in list(enumerate(graph_generate.unnamed_segments)) + [
         (x.name, x) for x in graph_generate.named_segments
     ]:
-        h_file.append(
-            "::std::unique_ptr<Path> %s();" % path_function_name(name))
-        cc_file.append(
-            "::std::unique_ptr<Path> %s() {" % path_function_name(name))
+        h_file.append("::std::unique_ptr<Path> %s();" %
+                      path_function_name(name))
+        cc_file.append("::std::unique_ptr<Path> %s() {" %
+                       path_function_name(name))
         cc_file.append("  return ::std::unique_ptr<Path>(new Path({")
         for point in segment.ToThetaPoints():
             cc_file.append("      {{%.12f, %.12f, %.12f," %
@@ -188,7 +190,8 @@
                    "::std::vector<TrajectoryAndParams> *trajectories,")
     cc_file.append("                            "
                    "const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
-    cc_file.append("                            " "double vmax) {")
+    cc_file.append("                            "
+                   "double vmax) {")
     cc_file.append("  ::std::vector<SearchGraph::Edge> edges;")
 
     index = 0
@@ -202,8 +205,8 @@
         add_edge(cc_file, name, segment, index, True)
         index += 1
 
-    cc_file.append("  return SearchGraph(%d, ::std::move(edges));" % len(
-        graph_generate.points))
+    cc_file.append("  return SearchGraph(%d, ::std::move(edges));" %
+                   len(graph_generate.points))
     cc_file.append("}")
 
     h_file.append("")
diff --git a/y2018/control_loops/python/graph_edit.py b/y2018/control_loops/python/graph_edit.py
index fa9c9e5..7b6179c 100644
--- a/y2018/control_loops/python/graph_edit.py
+++ b/y2018/control_loops/python/graph_edit.py
@@ -7,6 +7,7 @@
 import random
 import gi
 import numpy
+
 gi.require_version('Gtk', '3.0')
 from gi.repository import Gdk, Gtk
 import cairo
@@ -70,8 +71,8 @@
 
 # right hand side lines
 lines2 = [(joint_center[0] + derr, 0.3048), (0.422275, 0.3048),
-          (0.422275, 0.1397), (0.826135, 0.1397), (0.826135,
-                                                   inter_y(0.826135))]
+          (0.422275, 0.1397), (0.826135, 0.1397),
+          (0.826135, inter_y(0.826135))]
 
 t1_min = get_angle((32.525 - 4.0) * 0.0254)
 t2_min = -7.0 / 4.0 * numpy.pi
@@ -119,8 +120,8 @@
 
 p1 = Polygon(lines_theta)
 
-p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min), (t1_max, t2_max), (t1_min,
-                                                                     t2_max)])
+p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min), (t1_max, t2_max),
+              (t1_min, t2_max)])
 
 # Fully computed theta constrints.
 lines_theta = list(p1.intersection(p2).exterior.coords)
@@ -157,7 +158,6 @@
             abs(dpx * pdx + dpy * pdy)
 
 
-
 def closest_segment(lines, pt):
     c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
     for i in range(1, len(lines)):
@@ -172,6 +172,7 @@
 
 # Create a GTK+ widget on which we will draw using Cairo
 class Silly(basic_window.BaseWindow):
+
     def __init__(self):
         super(Silly, self).__init__()
 
@@ -230,6 +231,7 @@
             self.window.queue_draw()
 
     def method_connect(self, event, cb):
+
         def handler(obj, *args):
             cb(*args)
 
@@ -332,7 +334,8 @@
                 cr.stroke()
 
         if not self.theta_version:
-            theta1, theta2 = to_theta(self.last_pos, self.circular_index_select)
+            theta1, theta2 = to_theta(self.last_pos,
+                                      self.circular_index_select)
             x, y = joint_center[0], joint_center[1]
             cr.move_to(x, y)
 
@@ -473,16 +476,16 @@
         else:
             self.segments[0].control2 = self.now_segment_pt
 
-        print('Clicked at theta: %s' % (repr(self.now_segment_pt,)))
+        print('Clicked at theta: %s' % (repr(self.now_segment_pt, )))
         if not self.theta_version:
             print('Clicked at xy, circular index: (%f, %f, %f)' %
                   (self.last_pos[0], self.last_pos[1],
                    self.circular_index_select))
 
-        print('c1: numpy.array([%f, %f])' % (self.segments[0].control1[0],
-                                             self.segments[0].control1[1]))
-        print('c2: numpy.array([%f, %f])' % (self.segments[0].control2[0],
-                                             self.segments[0].control2[1]))
+        print('c1: numpy.array([%f, %f])' %
+              (self.segments[0].control1[0], self.segments[0].control1[1]))
+        print('c2: numpy.array([%f, %f])' %
+              (self.segments[0].control2[0], self.segments[0].control2[1]))
 
         self.redraw()
 
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index dae7caa..046b9dd 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -195,6 +195,7 @@
 
 # Segment in angle space.
 class AngleSegment:
+
     def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
         """Creates an angle segment.
 
@@ -321,8 +322,8 @@
     a = alpha_blend(start, control1, alpha)
     b = alpha_blend(control1, control2, alpha)
     c = alpha_blend(control2, end, alpha)
-    return alpha_blend(
-        alpha_blend(a, b, alpha), alpha_blend(b, c, alpha), alpha)
+    return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
+                       alpha)
 
 
 def subdivide_spline(start, control1, control2, end):
@@ -333,6 +334,7 @@
 
 
 class SplineSegment:
+
     def __init__(self,
                  start,
                  control1,
@@ -350,10 +352,9 @@
         self.vmax = vmax
 
     def __repr__(self):
-        return "SplineSegment(%s, %s, %s, %s)" % (repr(self.start),
-                                                  repr(self.control1),
-                                                  repr(self.control2),
-                                                  repr(self.end))
+        return "SplineSegment(%s, %s, %s, %s)" % (repr(
+            self.start), repr(self.control1), repr(
+                self.control2), repr(self.end))
 
     def DrawTo(self, cr, theta_version):
         if theta_version:
@@ -364,9 +365,8 @@
             end = get_xy(self.end)
 
             draw_lines(cr, [
-                to_theta(
-                    spline_eval(start, control1, control2, end, alpha),
-                    c_i_select)
+                to_theta(spline_eval(start, control1, control2, end, alpha),
+                         c_i_select)
                 for alpha in subdivide_spline(start, control1, control2, end)
             ])
             cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
@@ -418,6 +418,7 @@
 
 
 class ThetaSplineSegment:
+
     def __init__(self,
                  start,
                  control1,
@@ -435,10 +436,9 @@
         self.vmax = vmax
 
     def __repr__(self):
-        return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(self.start),
-                                                       repr(self.control1),
-                                                       repr(self.control2),
-                                                       repr(self.end))
+        return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
+            self.start), repr(self.control1), repr(
+                self.control2), repr(self.end))
 
     def DrawTo(self, cr, theta_version):
         if (theta_version):
@@ -485,30 +485,36 @@
 short_box_x = 0.431
 short_box_y = 0.082
 
-ready_above_box = to_theta_with_circular_index(
-    tall_box_x, tall_box_y + 0.08, circular_index=-1)
-tall_box_grab = to_theta_with_circular_index(
-    tall_box_x, tall_box_y, circular_index=-1)
-short_box_grab = to_theta_with_circular_index(
-    short_box_x, short_box_y, circular_index=-1)
+ready_above_box = to_theta_with_circular_index(tall_box_x,
+                                               tall_box_y + 0.08,
+                                               circular_index=-1)
+tall_box_grab = to_theta_with_circular_index(tall_box_x,
+                                             tall_box_y,
+                                             circular_index=-1)
+short_box_grab = to_theta_with_circular_index(short_box_x,
+                                              short_box_y,
+                                              circular_index=-1)
 
 # TODO(austin): Drive the front/back off the same numbers a bit better.
 front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
-front_middle3_box = to_theta_with_circular_index(
-    0.700, 2.125, circular_index=-1.000000)
-front_middle2_box = to_theta_with_circular_index(
-    0.700, 2.268, circular_index=-1)
-front_middle1_box = to_theta_with_circular_index(
-    0.800, 1.915, circular_index=-1)
+front_middle3_box = to_theta_with_circular_index(0.700,
+                                                 2.125,
+                                                 circular_index=-1.000000)
+front_middle2_box = to_theta_with_circular_index(0.700,
+                                                 2.268,
+                                                 circular_index=-1)
+front_middle1_box = to_theta_with_circular_index(0.800,
+                                                 1.915,
+                                                 circular_index=-1)
 front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
 back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
-back_middle2_box = to_theta_with_circular_index(
-    -0.700, 2.27, circular_index=0)
-back_middle1_box = to_theta_with_circular_index(
-    -0.800, 1.93, circular_index=0)
+back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
+back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
 back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
 
-back_extra_low_box = to_theta_with_circular_index(-0.87, 1.52, circular_index=0)
+back_extra_low_box = to_theta_with_circular_index(-0.87,
+                                                  1.52,
+                                                  circular_index=0)
 
 front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
 back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
@@ -517,26 +523,20 @@
 
 up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
 
-front_switch_auto = to_theta_with_circular_index(
-    0.750, 2.20, circular_index=-1.000000)
+front_switch_auto = to_theta_with_circular_index(0.750,
+                                                 2.20,
+                                                 circular_index=-1.000000)
 
-duck = numpy.array(
-    [numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
+duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
 
-starting = numpy.array(
-    [numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
-vertical_starting = numpy.array(
-    [numpy.pi / 2.0, -numpy.pi / 2.0])
+starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
+vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
 
-self_hang = numpy.array(
-    [numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
-partner_hang = numpy.array(
-    [numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
+self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
+partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
 
-above_hang = numpy.array(
-    [numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
-below_hang = numpy.array(
-    [numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
+above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
+below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
 
 up_c1 = to_theta((0.63, 1.17), circular_index=-1)
 up_c2 = to_theta((0.65, 1.62), circular_index=-1)
@@ -603,25 +603,25 @@
         num_points = int(numpy.ceil(norm / max_distance))
         last_iteration_point = previous_point
         for subindex in range(1, num_points):
-            subpoint = to_theta(
-                alpha_blend(previous_point_xy, point_xy,
-                            float(subindex) / num_points),
-                circular_index=circular_index)
-            result_points.append((subpoint, '%s%dof%d' % (name, subindex,
-                                                          num_points)))
+            subpoint = to_theta(alpha_blend(previous_point_xy, point_xy,
+                                            float(subindex) / num_points),
+                                circular_index=circular_index)
+            result_points.append(
+                (subpoint, '%s%dof%d' % (name, subindex, num_points)))
             result_paths.append(
                 XYSegment(last_iteration_point, subpoint, vmax=6.0))
             if (last_iteration_point != previous_point).any():
                 result_paths.append(XYSegment(previous_point, subpoint))
             if subindex == num_points - 1:
-              result_paths.append(XYSegment(subpoint, point, vmax=6.0))
+                result_paths.append(XYSegment(subpoint, point, vmax=6.0))
             else:
-              result_paths.append(XYSegment(subpoint, point))
+                result_paths.append(XYSegment(subpoint, point))
             last_iteration_point = subpoint
         result_points.append((point, name))
 
     return result_points, result_paths
 
+
 front_points, front_paths = expand_points(sparse_front_points, 0.06)
 back_points, back_paths = expand_points(sparse_back_points, 0.06)
 
@@ -650,7 +650,6 @@
 front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
 front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
 
-
 # We need to define critical points so we can create paths connecting them.
 # TODO(austin): Attach velocities to the slow ones.
 ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
@@ -670,30 +669,58 @@
 neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
 neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
 
-
 named_segments = [
-    ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2, back_switch, "BackSwitch"),
-
-    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2, back_high_box, "NeutralBoxToHigh", alpha_unitizer=long_alpha_unitizer),
-    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "NeutralBoxToMiddle2", long_alpha_unitizer),
-    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "NeutralBoxToMiddle1", long_alpha_unitizer),
-    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2, back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
-
-    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1, tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh", long_alpha_unitizer),
-    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "ReadyBoxToMiddle2", long_alpha_unitizer),
-    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "ReadyBoxToMiddle1", long_alpha_unitizer),
-    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1, tall_to_back_low_c2, back_low_box, "ReadyBoxToLow", long_alpha_unitizer),
-
-    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1, tall_to_back_high_c2, back_high_box, "ShortBoxToHigh", long_alpha_unitizer),
-    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "ShortBoxToMiddle2", long_alpha_unitizer),
-    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "ShortBoxToMiddle1", long_alpha_unitizer),
-    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1, tall_to_back_low_c2, back_low_box, "ShortBoxToLow", long_alpha_unitizer),
-
-    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_high_c2, back_high_box, "TallBoxToHigh", long_alpha_unitizer),
-    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_high_c2, back_middle2_box, "TallBoxToMiddle2", long_alpha_unitizer),
-    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2, back_middle1_box, "TallBoxToMiddle1", long_alpha_unitizer),
-    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2, back_low_box, "TallBoxToLow", long_alpha_unitizer),
-
+    ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
+                       back_switch, "BackSwitch"),
+    ThetaSplineSegment(neutral,
+                       neutral_to_back_low_c1,
+                       tall_to_back_high_c2,
+                       back_high_box,
+                       "NeutralBoxToHigh",
+                       alpha_unitizer=long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
+                       back_middle2_box, "NeutralBoxToMiddle2",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+                       back_middle1_box, "NeutralBoxToMiddle1",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+                       back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "ReadyBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_low_c2, back_middle1_box,
+                       "ReadyBoxToMiddle1", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "ShortBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_low_c2, back_middle1_box,
+                       "ShortBoxToMiddle1", long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "TallBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+                       back_middle1_box, "TallBoxToMiddle1",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+                       back_low_box, "TallBoxToLow", long_alpha_unitizer),
     SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
                   ready_above_box, "ReadyToNeutral"),
     XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
@@ -713,32 +740,29 @@
 ]
 
 unnamed_segments = [
-    SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2, front_switch_auto),
+    SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
+                  front_switch_auto),
     SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
     SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
     SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
     ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
     SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
-
     XYSegment(ready_above_box, front_low_box),
     XYSegment(ready_above_box, front_switch),
     XYSegment(ready_above_box, front_middle1_box),
     XYSegment(ready_above_box, front_middle2_box),
     XYSegment(ready_above_box, front_middle3_box),
-    SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, front_high_box),
-
+    SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
+                  front_high_box),
     AngleSegment(starting, vertical_starting),
     AngleSegment(vertical_starting, neutral),
-
     XYSegment(neutral, front_low_box),
     XYSegment(up, front_high_box),
     XYSegment(up, front_middle2_box),
-
     XYSegment(front_middle3_box, up),
     XYSegment(front_middle3_box, front_high_box),
     XYSegment(front_middle3_box, front_middle2_box),
     XYSegment(front_middle3_box, front_middle1_box),
-
     XYSegment(up, front_middle1_box),
     XYSegment(up, front_low_box),
     XYSegment(front_high_box, front_middle2_box),
@@ -760,7 +784,6 @@
     XYSegment(back_middle2_box, back_middle1_box),
     XYSegment(back_middle2_box, back_low_box),
     XYSegment(back_middle1_box, back_low_box),
-
     AngleSegment(up, above_hang),
     AngleSegment(above_hang, below_hang),
     AngleSegment(up, below_hang),
diff --git a/y2018/control_loops/python/intake.py b/y2018/control_loops/python/intake.py
index 00f737b..92b7c7a 100755
--- a/y2018/control_loops/python/intake.py
+++ b/y2018/control_loops/python/intake.py
@@ -105,8 +105,11 @@
         r_nm = 0.025
         self.R = numpy.matrix(numpy.diag([(r_nm**2.0), (r_nm**2.0)]))
 
-        self.KalmanGain, self.Q_steady = controls.kalman(
-            A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+        self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+                                                         B=self.B,
+                                                         C=self.C,
+                                                         Q=self.Q,
+                                                         R=self.R)
 
         # The box formed by U_min and U_max must encompass all possible values,
         # or else Austin's code gets angry.
@@ -181,8 +184,9 @@
 
         self.R = numpy.matrix([[(1.0 / (12.0**2.0))]])
 
-        self.K_transformed = controls.dlqr(
-            self.A_transformed, self.B_transformed, self.Q_lqr, self.R)
+        self.K_transformed = controls.dlqr(self.A_transformed,
+                                           self.B_transformed, self.Q_lqr,
+                                           self.R)
 
         # Write the controller back out in the absolute coordinate system.
         self.K = numpy.hstack(
@@ -215,8 +219,11 @@
                    repr(numpy.linalg.eig(self.A - self.B * self.K)[0]),
                    self._name)
 
-        self.KalmanGain, self.Q_steady = controls.kalman(
-            A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+        self.KalmanGain, self.Q_steady = controls.kalman(A=self.A,
+                                                         B=self.B,
+                                                         C=self.C,
+                                                         Q=self.Q,
+                                                         R=self.R)
 
         self.InitializeState()
 
@@ -359,11 +366,10 @@
     observer_intake.X_hat[0, 0] = intake.X[0, 0]
 
     # Test moving the intake with constant separation.
-    scenario_plotter.run_test(
-        intake,
-        controller_intake=intake_controller,
-        observer_intake=observer_intake,
-        iterations=200)
+    scenario_plotter.run_test(intake,
+                              controller_intake=intake_controller,
+                              observer_intake=observer_intake,
+                              iterations=200)
 
     if FLAGS.plot:
         scenario_plotter.Plot()
@@ -376,8 +382,8 @@
     else:
         namespaces = ['y2018', 'control_loops', 'superstructure', 'intake']
         intake = Intake('Intake')
-        loop_writer = control_loop.ControlLoopWriter(
-            'Intake', [intake], namespaces=namespaces)
+        loop_writer = control_loop.ControlLoopWriter('Intake', [intake],
+                                                     namespaces=namespaces)
         loop_writer.AddConstant(
             control_loop.Constant('kGearRatio', '%f', intake.G))
         loop_writer.AddConstant(
@@ -387,8 +393,9 @@
         loop_writer.Write(argv[1], argv[2])
 
         delayed_intake = DelayedIntake('DelayedIntake')
-        loop_writer = control_loop.ControlLoopWriter(
-            'DelayedIntake', [delayed_intake], namespaces=namespaces)
+        loop_writer = control_loop.ControlLoopWriter('DelayedIntake',
+                                                     [delayed_intake],
+                                                     namespaces=namespaces)
         loop_writer.Write(argv[3], argv[4])
 
 
diff --git a/y2018/control_loops/python/intake_simple.py b/y2018/control_loops/python/intake_simple.py
index 9b6ffb1..4d34f4b 100644
--- a/y2018/control_loops/python/intake_simple.py
+++ b/y2018/control_loops/python/intake_simple.py
@@ -21,253 +21,273 @@
 run_count = 0
 theta_travel = 0.0
 
+
 def to_deg(angle):
-  return angle * rad_to_deg
+    return angle * rad_to_deg
+
 
 def to_rad(angle):
-  return angle / rad_to_deg
+    return angle / rad_to_deg
+
 
 def to_rotations(angle):
-  return angle / pi2
+    return angle / pi2
+
 
 def time_derivative(x, t, voltage, c1, c2, c3):
-  global run_count
-  theta, omega = x
-  dxdt = [omega, -c1 * omega + c3 * math.sin(theta) + c2 * voltage]
-  run_count = run_count + 1
+    global run_count
+    theta, omega = x
+    dxdt = [omega, -c1 * omega + c3 * math.sin(theta) + c2 * voltage]
+    run_count = run_count + 1
 
-  return dxdt
+    return dxdt
+
 
 def get_distal_angle(theta_proximal):
-  # For the proximal angle = -50 degrees, the distal angle is -180 degrees
-  # For the proximal angle =  10 degrees, the distal angle is  -90 degrees
-  distal_angle = to_rad(-180.0 - (-50.0 - to_deg(theta_proximal)) * \
-      (180.0 - 90.0) / (50.0 + 10.0))
-  return distal_angle
+    # For the proximal angle = -50 degrees, the distal angle is -180 degrees
+    # For the proximal angle =  10 degrees, the distal angle is  -90 degrees
+    distal_angle = to_rad(-180.0 - (-50.0 - to_deg(theta_proximal)) * \
+        (180.0 - 90.0) / (50.0 + 10.0))
+    return distal_angle
 
 
 def get_180_degree_time(c1, c2, c3, voltage, gear_ratio, motor_free_speed):
-  global run_count
-  global theta_travel
+    global run_count
+    global theta_travel
 
-  if ( True ):
-    # Gravity is assisting the motion.
-    theta_start = 0.0
-    theta_target = pi
-  elif ( False ):
-    # Gravity is assisting the motion.
-    theta_start = 0.0
-    theta_target = -pi
-  elif ( False ):
-    # Gravity is slowing the motion.
-    theta_start = pi
-    theta_target = 0.0
-  elif ( False ):
-    # Gravity is slowing the motion.
-    theta_start = -pi
-    theta_target = 0.0
-  elif ( False ):
-    # This is for the proximal arm motion.
-    theta_start = to_rad(-50.0)
-    theta_target = to_rad(10.0)
+    if (True):
+        # Gravity is assisting the motion.
+        theta_start = 0.0
+        theta_target = pi
+    elif (False):
+        # Gravity is assisting the motion.
+        theta_start = 0.0
+        theta_target = -pi
+    elif (False):
+        # Gravity is slowing the motion.
+        theta_start = pi
+        theta_target = 0.0
+    elif (False):
+        # Gravity is slowing the motion.
+        theta_start = -pi
+        theta_target = 0.0
+    elif (False):
+        # This is for the proximal arm motion.
+        theta_start = to_rad(-50.0)
+        theta_target = to_rad(10.0)
 
-  theta_half = 0.5 * (theta_start + theta_target)
-  if theta_start > theta_target:
-    voltage = -voltage
-  theta = theta_start
-  theta_travel = theta_start - theta_target
-  if run_count == 0:
-    print("# Theta Start = %.2f radians End = %.2f Theta travel %.2f "
-          "Theta half = %.2f Voltage = %.2f" % (
-              theta_start, theta_target, theta_travel, theta_half, voltage))
-    print("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f "
-          "Theta half = %.2f Voltage = %.2f" % (
-              to_deg(theta_start), to_deg(theta_target), to_deg(theta_travel),
-              to_deg(theta_half), voltage))
-  omega = 0.0
-  time = 0.0
-  delta_time = 0.01 # time step in seconds
-  for step in range(1, 5000):
-     t = numpy.array([time, time + delta_time])
-     time = time + delta_time
-     x = [theta, omega]
-     angular_acceleration = -c1 * omega + c2 * voltage
-     x_n_plus_1 = scipy.integrate.odeint(time_derivative, x, t,
-                                         args=(voltage, c1, c2, c3))
-     theta, omega = x_n_plus_1[1]
+    theta_half = 0.5 * (theta_start + theta_target)
+    if theta_start > theta_target:
+        voltage = -voltage
+    theta = theta_start
+    theta_travel = theta_start - theta_target
+    if run_count == 0:
+        print("# Theta Start = %.2f radians End = %.2f Theta travel %.2f "
+              "Theta half = %.2f Voltage = %.2f" %
+              (theta_start, theta_target, theta_travel, theta_half, voltage))
+        print("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f "
+              "Theta half = %.2f Voltage = %.2f" %
+              (to_deg(theta_start), to_deg(theta_target), to_deg(theta_travel),
+               to_deg(theta_half), voltage))
+    omega = 0.0
+    time = 0.0
+    delta_time = 0.01  # time step in seconds
+    for step in range(1, 5000):
+        t = numpy.array([time, time + delta_time])
+        time = time + delta_time
+        x = [theta, omega]
+        angular_acceleration = -c1 * omega + c2 * voltage
+        x_n_plus_1 = scipy.integrate.odeint(time_derivative,
+                                            x,
+                                            t,
+                                            args=(voltage, c1, c2, c3))
+        theta, omega = x_n_plus_1[1]
 
-     if ( False ):
-       print("%4d  %8.4f %8.2f          %8.4f          %8.4f    %8.3f    "
-             "%8.3f     %8.3f      %8.3f" % (
-                 step, time, theta, omega, angular_acceleration,
-                 to_rotations(theta), to_rotations(omega),
-                 omega * gear_ratio * 60.0 / pi2,
-                 omega * gear_ratio / motor_free_speed))
-     if theta_start < theta_target:
-       # Angle is increasing through the motion.
-       if theta > theta_half:
-         break
-     else:
-       # Angle is decreasing through the motion.
-       if (theta < theta_half):
-         break
+        if (False):
+            print(
+                "%4d  %8.4f %8.2f          %8.4f          %8.4f    %8.3f    "
+                "%8.3f     %8.3f      %8.3f" %
+                (step, time, theta, omega, angular_acceleration,
+                 to_rotations(theta), to_rotations(omega), omega * gear_ratio *
+                 60.0 / pi2, omega * gear_ratio / motor_free_speed))
+        if theta_start < theta_target:
+            # Angle is increasing through the motion.
+            if theta > theta_half:
+                break
+        else:
+            # Angle is decreasing through the motion.
+            if (theta < theta_half):
+                break
 
-  return 2.0 * time
+    return 2.0 * time
+
 
 def main():
-  # m/sec^2 Gravity Constant
-  gravity = 9.8
-  # m/sec^2 Gravity Constant - Use 0.0 for the intake.  It is horizontal.
-  gravity = 0.0
-  # Volts
-  voltage_nominal = 12
+    # m/sec^2 Gravity Constant
+    gravity = 9.8
+    # m/sec^2 Gravity Constant - Use 0.0 for the intake.  It is horizontal.
+    gravity = 0.0
+    # Volts
+    voltage_nominal = 12
 
-  # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
-  motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
-  current_stall = 134 # amps stall current
-  current_no_load = 0.7 # amps no load current
-  torque_stall = 710/1000.0 # N-m Stall Torque
-  speed_no_load_rpm = 18730 # RPM no load speed
+    # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
+    motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
+    current_stall = 134  # amps stall current
+    current_no_load = 0.7  # amps no load current
+    torque_stall = 710 / 1000.0  # N-m Stall Torque
+    speed_no_load_rpm = 18730  # RPM no load speed
 
-  if ( True ):
-    # Bag motor from https://www.vexrobotics.com/217-3351.html
-    motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
-    current_stall = 53.0 # amps stall current
-    current_no_load = 1.8 # amps no load current
-    torque_stall = 0.4 # N-m Stall Torque
-    speed_no_load_rpm = 13180.0 # RPM no load speed
+    if (True):
+        # Bag motor from https://www.vexrobotics.com/217-3351.html
+        motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
+        current_stall = 53.0  # amps stall current
+        current_no_load = 1.8  # amps no load current
+        torque_stall = 0.4  # N-m Stall Torque
+        speed_no_load_rpm = 13180.0  # RPM no load speed
 
-  if ( False ):
-    # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
-    motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
-    current_stall = 89.0 # amps stall current
-    current_no_load = 3.0 # amps no load current
-    torque_stall = 1.4 # N-m Stall Torque
-    speed_no_load_rpm = 5840.0 # RPM no load speed
+    if (False):
+        # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
+        motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
+        current_stall = 89.0  # amps stall current
+        current_no_load = 3.0  # amps no load current
+        torque_stall = 1.4  # N-m Stall Torque
+        speed_no_load_rpm = 5840.0  # RPM no load speed
 
-  # How many motors are we using?
-  num_motors = 1
+    # How many motors are we using?
+    num_motors = 1
 
-  # Motor values
-  print("# Motor: %s" % (motor_name))
-  print("# Number of motors: %d" % (num_motors))
-  print("# Stall torque: %.1f n-m" % (torque_stall))
-  print("# Stall current: %.1f amps" % (current_stall))
-  print("# No load current: %.1f amps" % (current_no_load))
-  print("# No load speed: %.0f rpm" % (speed_no_load_rpm))
+    # Motor values
+    print("# Motor: %s" % (motor_name))
+    print("# Number of motors: %d" % (num_motors))
+    print("# Stall torque: %.1f n-m" % (torque_stall))
+    print("# Stall current: %.1f amps" % (current_stall))
+    print("# No load current: %.1f amps" % (current_no_load))
+    print("# No load speed: %.0f rpm" % (speed_no_load_rpm))
 
-  # Constants from motor values
-  resistance_motor = voltage_nominal / current_stall
-  speed_no_load_rps = speed_no_load_rpm / 60.0 # Revolutions per second no load speed
-  speed_no_load = speed_no_load_rps * 2.0 * pi
-  Kt = num_motors * torque_stall / current_stall # N-m/A torque constant
-  Kv_rpm = speed_no_load_rpm / (voltage_nominal -
-                                resistance_motor * current_no_load)  # rpm/V
-  Kv = Kv_rpm * 2.0 * pi / 60.0  # rpm/V
+    # Constants from motor values
+    resistance_motor = voltage_nominal / current_stall
+    speed_no_load_rps = speed_no_load_rpm / 60.0  # Revolutions per second no load speed
+    speed_no_load = speed_no_load_rps * 2.0 * pi
+    Kt = num_motors * torque_stall / current_stall  # N-m/A torque constant
+    Kv_rpm = speed_no_load_rpm / (
+        voltage_nominal - resistance_motor * current_no_load)  # rpm/V
+    Kv = Kv_rpm * 2.0 * pi / 60.0  # rpm/V
 
-  # Robot Geometry and physics
-  # m Length of arm connected to the robot base
-  length_proximal_arm = inches_to_meters * 47.34
-  # m Length of arm that holds the cube
-  length_distal_arm = inches_to_meters * 44.0
-  # m Length of intake arm from the pivot point to where the big roller contacts a cube.
-  length_intake_arm =  inches_to_meters * 9.0
-  mass_cube = 6.0 * lbs_to_kg  # Weight of the cube in Kgrams
-  mass_proximal_arm = 5.5 * lbs_to_kg # Weight of proximal arm
-  mass_distal_arm = 3.5 * lbs_to_kg # Weight of distal arm
-  mass_distal = mass_cube + mass_distal_arm
-  mass_proximal = mass_proximal_arm + mass_distal
-  # m Length from arm pivot point to arm CG
-  radius_to_proximal_arm_cg = 22.0 * inches_to_meters
-  # m Length from arm pivot point to arm CG
-  radius_to_distal_arm_cg = 10.0 * inches_to_meters
+    # Robot Geometry and physics
+    # m Length of arm connected to the robot base
+    length_proximal_arm = inches_to_meters * 47.34
+    # m Length of arm that holds the cube
+    length_distal_arm = inches_to_meters * 44.0
+    # m Length of intake arm from the pivot point to where the big roller contacts a cube.
+    length_intake_arm = inches_to_meters * 9.0
+    mass_cube = 6.0 * lbs_to_kg  # Weight of the cube in Kgrams
+    mass_proximal_arm = 5.5 * lbs_to_kg  # Weight of proximal arm
+    mass_distal_arm = 3.5 * lbs_to_kg  # Weight of distal arm
+    mass_distal = mass_cube + mass_distal_arm
+    mass_proximal = mass_proximal_arm + mass_distal
+    # m Length from arm pivot point to arm CG
+    radius_to_proximal_arm_cg = 22.0 * inches_to_meters
+    # m Length from arm pivot point to arm CG
+    radius_to_distal_arm_cg = 10.0 * inches_to_meters
 
-  radius_to_distal_cg = (length_distal_arm * mass_cube +
-                         radius_to_distal_arm_cg * mass_distal_arm) / \
-                             mass_distal
-  radius_to_proximal_cg = (length_proximal_arm * mass_distal +
-                           radius_to_proximal_arm_cg * mass_proximal_arm) / \
-                               mass_proximal
-  J_cube = length_distal_arm * length_distal_arm*mass_cube
-  # Kg m^2 Moment of inertia of the proximal arm
-  J_proximal_arm = radius_to_proximal_arm_cg * radius_to_proximal_arm_cg * \
-      mass_distal_arm
-  # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
-  J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm * \
-      length_proximal_arm * mass_distal
-  # Kg m^2 Moment of inertia of the distal arm
-  J_distal_arm = radius_to_distal_arm_cg * radius_to_distal_arm_cg * mass_distal_arm
-  # Moment of inertia of the arm with the cube on the end
-  J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm
-  # Intake claw
-  J_intake = 0.295  # Kg m^2 Moment of inertia of intake
-  J = J_intake
+    radius_to_distal_cg = (length_distal_arm * mass_cube +
+                           radius_to_distal_arm_cg * mass_distal_arm) / \
+                               mass_distal
+    radius_to_proximal_cg = (length_proximal_arm * mass_distal +
+                             radius_to_proximal_arm_cg * mass_proximal_arm) / \
+                                 mass_proximal
+    J_cube = length_distal_arm * length_distal_arm * mass_cube
+    # Kg m^2 Moment of inertia of the proximal arm
+    J_proximal_arm = radius_to_proximal_arm_cg * radius_to_proximal_arm_cg * \
+        mass_distal_arm
+    # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
+    J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm * \
+        length_proximal_arm * mass_distal
+    # Kg m^2 Moment of inertia of the distal arm
+    J_distal_arm = radius_to_distal_arm_cg * radius_to_distal_arm_cg * mass_distal_arm
+    # Moment of inertia of the arm with the cube on the end
+    J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm
+    # Intake claw
+    J_intake = 0.295  # Kg m^2 Moment of inertia of intake
+    J = J_intake
 
-  gear_ratio = 140.0  # Guess at the gear ratio
-  gear_ratio = 100.0  # Guess at the gear ratio
-  gear_ratio = 90.0  # Guess at the gear ratio
+    gear_ratio = 140.0  # Guess at the gear ratio
+    gear_ratio = 100.0  # Guess at the gear ratio
+    gear_ratio = 90.0  # Guess at the gear ratio
 
-  error_margine = 1.0
-  voltage = 10.0  # voltage for the motor.  Assuming a loaded robot so not using 12 V.
-  # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
-  # motor_free_speed = Kv * voltage
-  motor_free_speed = speed_no_load
+    error_margine = 1.0
+    voltage = 10.0  # voltage for the motor.  Assuming a loaded robot so not using 12 V.
+    # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
+    # motor_free_speed = Kv * voltage
+    motor_free_speed = speed_no_load
 
-  print("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" % (Kt, Kv_rpm, Kv))
-  print("# %.2f Ohms Resistance of the motor " % (resistance_motor))
-  print("# %.2f kg Cube weight" % (mass_cube))
-  print("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
-  print("# %.2f kg Distal Arm mass" % (mass_distal_arm))
-  print("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
-  print("# %.2f m Length from distal arm pivot point to arm CG" % (
-      radius_to_distal_arm_cg))
-  print("# %.2f m Length from distal arm pivot point to arm and cube cg" % (
-      radius_to_distal_cg))
-  print("# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point" % (J_cube))
-  print("# %.2f m Length from proximal arm pivot point to arm CG" % (radius_to_proximal_arm_cg))
-  print("# %.2f m Length from proximal arm pivot point to arm and cube cg" % (
-      radius_to_proximal_cg))
-  print("# %.2f m  Proximal arm length" % (length_proximal_arm))
-  print("# %.2f m  Distal arm length" % (length_distal_arm))
+    print("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" %
+          (Kt, Kv_rpm, Kv))
+    print("# %.2f Ohms Resistance of the motor " % (resistance_motor))
+    print("# %.2f kg Cube weight" % (mass_cube))
+    print("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
+    print("# %.2f kg Distal Arm mass" % (mass_distal_arm))
+    print("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
+    print("# %.2f m Length from distal arm pivot point to arm CG" %
+          (radius_to_distal_arm_cg))
+    print("# %.2f m Length from distal arm pivot point to arm and cube cg" %
+          (radius_to_distal_cg))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point"
+        % (J_cube))
+    print("# %.2f m Length from proximal arm pivot point to arm CG" %
+          (radius_to_proximal_arm_cg))
+    print("# %.2f m Length from proximal arm pivot point to arm and cube cg" %
+          (radius_to_proximal_cg))
+    print("# %.2f m  Proximal arm length" % (length_proximal_arm))
+    print("# %.2f m  Distal arm length" % (length_distal_arm))
 
-  print("# %.2f kg-m^2 Moment of inertia of the intake about the intake pivot point" % (
-      J_intake))
-  print("# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point" % (
-      J_distal_arm))
-  print("# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point" % (
-      J_proximal_arm))
-  print("# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about "
-        "the proximal arm pivot point" % (
-            J_distal_arm_and_cube_at_end_of_proximal_arm))
-  print("# %.2f kg-m^2 Moment of inertia of the intake the intake pivot point "
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the intake about the intake pivot point"
+        % (J_intake))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point"
+        % (J_distal_arm))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point"
+        % (J_proximal_arm))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about "
+        "the proximal arm pivot point" %
+        (J_distal_arm_and_cube_at_end_of_proximal_arm))
+    print(
+        "# %.2f kg-m^2 Moment of inertia of the intake the intake pivot point "
         "(J value used in simulation)" % (J))
-  print("# %d Number of motors" % (num_motors))
+    print("# %d Number of motors" % (num_motors))
 
-  print("# %.2f V Motor voltage" % (voltage))
-  for gear_ratio in range(60, 241, 10):
-    c1 = Kt * gear_ratio * gear_ratio / (Kv * resistance_motor * J)
-    c2 = gear_ratio * Kt / (J * resistance_motor)
-    c3 = radius_to_proximal_cg * mass_proximal * gravity / J
+    print("# %.2f V Motor voltage" % (voltage))
+    for gear_ratio in range(60, 241, 10):
+        c1 = Kt * gear_ratio * gear_ratio / (Kv * resistance_motor * J)
+        c2 = gear_ratio * Kt / (J * resistance_motor)
+        c3 = radius_to_proximal_cg * mass_proximal * gravity / J
 
-    if ( False ):
-      print("# %.8f 1/sec C1 constant" % (c1))
-      print("# %.2f 1/sec C2 constant" % (c2))
-      print("# %.2f 1/(V sec^2) C3 constant" % (c3))
-      print("# %.2f RPM Free speed at motor voltage" % (voltage * Kv_rpm))
+        if (False):
+            print("# %.8f 1/sec C1 constant" % (c1))
+            print("# %.2f 1/sec C2 constant" % (c2))
+            print("# %.2f 1/(V sec^2) C3 constant" % (c3))
+            print("# %.2f RPM Free speed at motor voltage" %
+                  (voltage * Kv_rpm))
 
-    torque_90_degrees = radius_to_distal_cg * mass_distal * gravity
-    voltage_90_degrees = resistance_motor * torque_90_degrees / (gear_ratio * Kt)
-    torque_peak = gear_ratio * num_motors * torque_stall
-    torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
-    normal_force = torque_peak / length_intake_arm
-    normal_force_lbf = newton_to_lbf * normal_force
-    time_required = get_180_degree_time(c1, c2, c3, voltage,
-                                        gear_ratio, motor_free_speed)
-    print("Time for %.1f degrees for gear ratio %3.0f is %.2f seconds.  "
-          "Peak (stall) torque %3.0f nm %3.0f ft-lb Normal force at intake "
-          "end %3.0f N %2.0f lbf" % \
-      (to_deg(theta_travel), gear_ratio, time_required,
-       torque_peak, torque_peak_ft_lbs, normal_force, normal_force_lbf))
+        torque_90_degrees = radius_to_distal_cg * mass_distal * gravity
+        voltage_90_degrees = resistance_motor * torque_90_degrees / (
+            gear_ratio * Kt)
+        torque_peak = gear_ratio * num_motors * torque_stall
+        torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
+        normal_force = torque_peak / length_intake_arm
+        normal_force_lbf = newton_to_lbf * normal_force
+        time_required = get_180_degree_time(c1, c2, c3, voltage, gear_ratio,
+                                            motor_free_speed)
+        print("Time for %.1f degrees for gear ratio %3.0f is %.2f seconds.  "
+              "Peak (stall) torque %3.0f nm %3.0f ft-lb Normal force at intake "
+              "end %3.0f N %2.0f lbf" % \
+          (to_deg(theta_travel), gear_ratio, time_required,
+           torque_peak, torque_peak_ft_lbs, normal_force, normal_force_lbf))
+
 
 if __name__ == '__main__':
-  main()
+    main()
diff --git a/y2018/control_loops/python/path_points.py b/y2018/control_loops/python/path_points.py
index eed9804..8802629 100644
--- a/y2018/control_loops/python/path_points.py
+++ b/y2018/control_loops/python/path_points.py
@@ -2,9 +2,422 @@
 
 # Path points from Parker.
 # Points in (theta0, theta1) of the boundry of the safe region.
-points_bounary = ([1.8577014383575772, -1.7353804562372057], [1.8322288438826315, -1.761574570216062], [1.7840339881582052, -1.8122752826851365], [1.7367354460218392, -1.863184376466228], [1.69054633425053, -1.9140647421906793], [1.6456590387358871, -1.9646939485526782], [1.6022412524081968, -2.0148691793459164], [1.5604334435784202, -2.0644108218872432], [1.5203477477575325, -2.1131646494909866], [1.4820681545944325, -2.1610026706370666], [1.4456517763321752, -2.207822815007094], [1.4111309389855604, -2.253547685496041], [1.3785158286698027, -2.298122627340264], [1.3477974448369014, -2.3415133576238922], [1.318950649522341, -2.3837033700108243], [1.2919371475531436, -2.4246912899477153], [1.266708279449626, -2.464488312575651], [1.2432075513427807, -2.5031158147261996], [1.2213728618107609, -2.5406031969824228], [1.2011384131259828, -2.5769859833310096], [1.1824363142639513, -2.61230418457078], [1.165197896140849, -2.64660091671642], [1.149354767212547, -2.6799212560849437], [1.134839641107821, -2.712311307404836], [1.121586968578607, -2.7438174590386177], [1.1095334047223224, -2.774485799309971], [1.09861813993779, -2.8043616692157007], [1.088783119985477, -2.8334893289039758], [1.079973177230731, -2.861911717797479], [1.0721360919151535, -2.8896702908486835], [1.065222599283597, -2.916804915950962], [1.0591863556764607, -2.943353819884641], [1.0539838743128325, -2.969353572295195], [1.049574439440948, -2.9948390990606946], [1.0459200058000553, -3.01984371800932], [1.0429850888932353, -3.044399191310831], [1.0407366503803421, -3.0685357900111128], [1.039143981929867, -3.092282367132034], [1.0381785900853944, -3.1156664365461078], [1.0378140840766794, -3.1387142554815886], [1.038026068010855, -3.1614509090414518], [1.0387920384931424, -3.1839003955494407], [1.0400912884293725, -3.2060857118856374], [1.0419048175385754, -3.2280289382581637], [1.0442152499395827, -3.2497513220895704], [1.047006759060362, -3.2712733608874625], [1.050265000044113, -3.2926148841283163], [1.0513266200838918, -3.2986722862692828], [0.6882849734317291, -3.2986722862692828], [0.6882849734317291, -2.40847516476558], [0.8062364039734171, -2.2816832357742984], [0.9846964491122989, -2.0749837238115147], [1.080856081125841, -1.9535526052833936], [1.1403399741223543, -1.8700303125904767], [1.1796460643255697, -1.8073338252603661], [1.206509908068604, -1.7574623871869075], [1.225108933139178, -1.7160975113819317], [1.237917343016644, -1.6806816402603253], [1.2465009152225068, -1.6495957958330445], [1.251901644035212, -1.6217619064422375], [1.2548410275662123, -1.5964327471863136], [1.2558349967266738, -1.5730732293972975], [1.2552624664807475, -1.551289614657788], [1.2534080548485127, -1.5307854126408047], [1.2504896957865235, -1.5113328783804112], [1.2466770509718135, -1.492754008779478], [1.2421041193998992, -1.4749075280685116], [1.236878076935188, -1.457679763958034], [1.231085601347444, -1.4409781183381307], [1.2247974811461413, -1.4247263085140511], [1.2180720288351026, -1.408860841660136], [1.2109576458572935, -1.3933283641188086], [1.2034947755974974, -1.378083641634472], [1.1957174082841977, -1.363088001457586], [1.1876542532510088, -1.3483081171759035], [1.179329661157153, -1.3337150510329991], [1.1707643560768641, -1.3192834919003447], [1.1385726725405734, -1.3512941162901886], [1.1061744838535637, -1.3828092440478137], [1.0736355415504857, -1.4137655512448706], [1.0410203155384732, -1.444102884084807], [1.0083912519665894, -1.4737649313813326], [0.975808099297045, -1.5026998012630925], [0.9433273192311136, -1.5308604887780404], [0.941597772428203, -1.50959779639341], [0.9392183822389457, -1.488861961175901], [0.9362399962318921, -1.4685999567644576], [0.9327074201772598, -1.448764371832554], [0.9286602163651203, -1.4293126388801052], [0.9241333769591611, -1.410206381002334], [0.9191578939466147, -1.3914108561164176], [0.9137612431353617, -1.3728944819981919], [0.9079677963791273, -1.3546284285619337], [0.9017991735984072, -1.3365862662768213], [0.8952745440670551, -1.3187436615861219], [0.888410884742945, -1.3010781117818295], [0.8812232020507231, -1.2835687130679965], [0.8737247224089426, -1.2661959565824437], [0.8659270558807739, -1.2489415479874506], [0.8578403365760008, -1.2317882469234374], [0.8810761292150717, -1.2118184809610988], [0.9041762360363244, -1.1914283310662528], [0.9271196683064211, -1.1706361926784383], [0.949885054142765, -1.1494613527499984], [0.9724507572661958, -1.1279238910069835], [0.9947950004998649, -1.1060445714951375], [1.0168959923240788, -1.0838447258650978], [1.0387320546908576, -1.0613461300367468], [1.0602817502430675, -1.0385708760262022], [1.0815240070740941, -1.0155412408147642], [1.102438239206001, -0.9922795541836177], [1.123004461055328, -0.9688080674301042], [1.1432033942926907, -0.9451488248218312], [1.1630165656794818, -0.9213235395370803], [1.1824263946752058, -0.8973534756890716], [1.2014162698440272, -0.8732593378443982], [1.2199706133398625, -0.8490611692304734], [1.2380749330068292, -0.8247782595916947], [1.2557158618869284, -0.800429063408306], [1.2728811851714203, -0.776031128944234], [1.2895598548592355, -0.7516010383486005], [1.3057419925890068, -0.7271543588072293], [1.3214188812865908, -0.702705604531139], [1.3365829464142562, -0.6782682091832445], [1.351227727719687, -0.6538545081853449], [1.365347842462401, -0.6294757302167044], [1.3789389411433586, -0.6051419971134862], [1.391997656782351, -0.5808623313043475], [1.4045215487801634, -0.5566446698699925], [1.4165090423718034, -0.5324958842910054], [1.4279593646268474, -0.5084218049460658], [1.4388724778869602, -0.4844272494383845], [1.449249011452494, -0.460516053858597], [1.4590901922431447, -0.4366911061340692], [1.468397775065033, -0.4129543806643518], [1.4771739730209745, -0.38930697349737264], [1.485421388504271, -0.36574913735813025], [1.4931429451209484, -0.3422803158986589], [1.5003418207921726, -0.3188991765927855], [1.5070213821985838, -0.2956036417497373], [1.513185120641734, -0.2723909171654731], [1.5188365893149296, -0.24925751796822435], [1.5239793418959948, -0.22619929124396096], [1.5286168722972349, -0.2032114350471563], [1.5327525553319497, -0.1802885134112242], [1.5363895879810432, -0.15742446697009113], [1.5395309308654115, -0.1346126187862511], [1.5421792494481048, -0.11184567494963411], [1.5443368544016174, -0.0891157194637005], [1.5460056404769755, -0.06641420286771664], [1.5471870230984175, -0.043731923953761714], [1.547881871775246, -0.021059003819238205], [1.5480904392645911, 0.0016151486553661097], [1.547812285227133, 0.024301881005710235], [1.5470461928818935, 0.047013352405288866], [1.5457900768723736, 0.06976260156314103], [1.5440408801865422, 0.09256362934244797], [1.5417944575035705, 0.11543149864415554], [1.5390454417383017, 0.13838245474060726], [1.5357870897759938, 0.16143407007284788], [1.5320111023738603, 0.18460541860588778], [1.5277074118667517, 0.20791728625864334], [1.5228639295308124, 0.23139242581719505], [1.5174662420569858, 0.25505586728497265], [1.5114972433117844, 0.27893529808946027], [1.5049366830391921, 0.30306153234932315], [1.4977606078174102, 0.32746909510770755], [1.4899406605544634, 0.35219695697813347], [1.4814431917184283, 0.37728946847450484], [1.4722281161523716, 0.40279756372788583], [1.4622474200998372, 0.4287803341537376], [1.4514431778273647, 0.45530712040457033], [1.4397448652396179, 0.48246034696312606], [1.427065639662639, 0.5103394485717625], [1.4132970536897833, 0.5390664502570618], [1.3983013135749314, 0.5687941401756967], [1.3818995257862978, 0.5997184788509978], [1.3638530549860057, 0.6320982830132342], [1.3438323057823602, 0.6662881915757862], [1.3213606855701236, 0.7027978462604986], [1.2957042956404132, 0.7424084023365868], [1.2656247456808565, 0.786433646353686], [1.2287034601894442, 0.8374338794107618], [1.1786904071656892, 0.902017503822168], [1.0497616572686415, 1.0497616572686426], [0.9097522267255194, 1.1864251300690412], [0.8484431816837388, 1.2397127624624213], [0.8000049977037023, 1.2791960970308718], [0.7581818853313602, 1.3114777786351866], [0.7205493452982701, 1.3391121846078957], [0.6882849734317291, 1.3617105524217008], [0.6882849734317291, 2.356194490192345], [1.3376784164442164, 2.356194490192345], [1.3516056511178856, 2.3360528189227487], [1.3708660530144583, 2.308792458458483], [1.3913233553973305, 2.2804469909122105], [1.4131687110627962, 2.2508089759388485], [1.436656614785, 2.219604401551449], [1.4621380338543308, 2.18645769238238], [1.4901198983671067, 2.1508288797419346], [1.5213822452925796, 2.111889875368928], [1.5572408030205178, 2.0682471032649676], [1.6002650085871786, 2.0171829478543404], [1.657096323745671, 1.9516778011159774], [1.7977393629734166, 1.7977393629734166], [1.8577014383575772, 1.7364646168980775], [1.8577014383575772, -1.7353804562372057])
+points_bounary = (
+    [1.8577014383575772,
+     -1.7353804562372057], [1.8322288438826315, -1.761574570216062],
+    [1.7840339881582052,
+     -1.8122752826851365], [1.7367354460218392, -1.863184376466228
+                            ], [1.69054633425053, -1.9140647421906793],
+    [1.6456590387358871, -1.9646939485526782
+     ], [1.6022412524081968, -2.0148691793459164
+         ], [1.5604334435784202, -2.0644108218872432
+             ], [1.5203477477575325, -2.1131646494909866
+                 ], [1.4820681545944325, -2.1610026706370666
+                     ], [1.4456517763321752, -2.207822815007094
+                         ], [1.4111309389855604, -2.253547685496041
+                             ], [1.3785158286698027, -2.298122627340264],
+    [1.3477974448369014, -2.3415133576238922
+     ], [1.318950649522341, -2.3837033700108243
+         ], [1.2919371475531436, -2.4246912899477153
+             ], [1.266708279449626, -2.464488312575651
+                 ], [1.2432075513427807, -2.5031158147261996
+                     ], [1.2213728618107609, -2.5406031969824228
+                         ], [1.2011384131259828, -2.5769859833310096
+                             ], [1.1824363142639513, -2.61230418457078
+                                 ], [1.165197896140849, -2.64660091671642],
+    [1.149354767212547, -2.6799212560849437
+     ], [1.134839641107821, -2.712311307404836
+         ], [1.121586968578607, -2.7438174590386177
+             ], [1.1095334047223224, -2.774485799309971
+                 ], [1.09861813993779, -2.8043616692157007
+                     ], [1.088783119985477, -2.8334893289039758
+                         ], [1.079973177230731, -2.861911717797479
+                             ], [1.0721360919151535, -2.8896702908486835
+                                 ], [1.065222599283597, -2.916804915950962],
+    [1.0591863556764607, -2.943353819884641
+     ], [1.0539838743128325, -2.969353572295195
+         ], [1.049574439440948, -2.9948390990606946
+             ], [1.0459200058000553, -3.01984371800932
+                 ], [1.0429850888932353, -3.044399191310831
+                     ], [1.0407366503803421, -3.0685357900111128
+                         ], [1.039143981929867, -3.092282367132034
+                             ], [1.0381785900853944, -3.1156664365461078
+                                 ], [1.0378140840766794, -3.1387142554815886],
+    [1.038026068010855,
+     -3.1614509090414518], [1.0387920384931424, -3.1839003955494407], [
+         1.0400912884293725,
+         -3.2060857118856374
+     ], [1.0419048175385754, -3.2280289382581637
+         ], [1.0442152499395827, -3.2497513220895704
+             ], [1.047006759060362, -3.2712733608874625
+                 ], [1.050265000044113, -3.2926148841283163
+                     ], [1.0513266200838918, -3.2986722862692828
+                         ], [0.6882849734317291, -3.2986722862692828
+                             ], [0.6882849734317291, -2.40847516476558
+                                 ], [0.8062364039734171, -2.2816832357742984],
+    [0.9846964491122989,
+     -2.0749837238115147], [1.080856081125841, -1.9535526052833936], [
+         1.1403399741223543,
+         -1.8700303125904767
+     ], [1.1796460643255697,
+         -1.8073338252603661], [1.206509908068604, -1.7574623871869075], [
+             1.225108933139178,
+             -1.7160975113819317
+         ], [1.237917343016644, -1.6806816402603253
+             ], [1.2465009152225068, -1.6495957958330445
+                 ], [1.251901644035212, -1.6217619064422375
+                     ], [1.2548410275662123, -1.5964327471863136
+                         ], [1.2558349967266738, -1.5730732293972975
+                             ], [1.2552624664807475, -1.551289614657788
+                                 ], [1.2534080548485127, -1.5307854126408047],
+    [1.2504896957865235,
+     -1.5113328783804112], [1.2466770509718135, -1.492754008779478], [
+         1.2421041193998992,
+         -1.4749075280685116
+     ], [1.236878076935188, -1.457679763958034
+         ], [1.231085601347444, -1.4409781183381307
+             ], [1.2247974811461413, -1.4247263085140511
+                 ], [1.2180720288351026, -1.408860841660136
+                     ], [1.2109576458572935, -1.3933283641188086
+                         ], [1.2034947755974974, -1.378083641634472
+                             ], [1.1957174082841977, -1.363088001457586
+                                 ], [1.1876542532510088, -1.3483081171759035],
+    [1.179329661157153,
+     -1.3337150510329991], [1.1707643560768641, -1.3192834919003447], [
+         1.1385726725405734,
+         -1.3512941162901886
+     ], [1.1061744838535637,
+         -1.3828092440478137], [1.0736355415504857, -1.4137655512448706], [
+             1.0410203155384732,
+             -1.444102884084807
+         ], [1.0083912519665894,
+             -1.4737649313813326], [0.975808099297045, -1.5026998012630925], [
+                 0.9433273192311136,
+                 -1.5308604887780404
+             ], [0.941597772428203, -1.50959779639341
+                 ], [0.9392183822389457, -1.488861961175901
+                     ], [0.9362399962318921, -1.4685999567644576
+                         ], [0.9327074201772598, -1.448764371832554
+                             ], [0.9286602163651203, -1.4293126388801052
+                                 ], [0.9241333769591611, -1.410206381002334],
+    [0.9191578939466147,
+     -1.3914108561164176], [0.9137612431353617, -1.3728944819981919], [
+         0.9079677963791273,
+         -1.3546284285619337
+     ], [0.9017991735984072,
+         -1.3365862662768213], [0.8952745440670551, -1.3187436615861219], [
+             0.888410884742945,
+             -1.3010781117818295
+         ], [0.8812232020507231,
+             -1.2835687130679965], [0.8737247224089426, -1.2661959565824437], [
+                 0.8659270558807739,
+                 -1.2489415479874506
+             ], [0.8578403365760008, -1.2317882469234374
+                 ], [0.8810761292150717, -1.2118184809610988
+                     ], [0.9041762360363244, -1.1914283310662528
+                         ], [0.9271196683064211, -1.1706361926784383
+                             ], [0.949885054142765, -1.1494613527499984
+                                 ], [0.9724507572661958, -1.1279238910069835],
+    [0.9947950004998649,
+     -1.1060445714951375], [1.0168959923240788, -1.0838447258650978], [
+         1.0387320546908576,
+         -1.0613461300367468
+     ], [1.0602817502430675,
+         -1.0385708760262022], [1.0815240070740941, -1.0155412408147642], [
+             1.102438239206001,
+             -0.9922795541836177
+         ], [1.123004461055328,
+             -0.9688080674301042], [1.1432033942926907, -0.9451488248218312], [
+                 1.1630165656794818,
+                 -0.9213235395370803
+             ], [1.1824263946752058, -0.8973534756890716
+                 ], [1.2014162698440272, -0.8732593378443982
+                     ], [1.2199706133398625, -0.8490611692304734
+                         ], [1.2380749330068292, -0.8247782595916947
+                             ], [1.2557158618869284, -0.800429063408306
+                                 ], [1.2728811851714203, -0.776031128944234],
+    [1.2895598548592355,
+     -0.7516010383486005], [1.3057419925890068, -0.7271543588072293], [
+         1.3214188812865908,
+         -0.702705604531139
+     ], [1.3365829464142562,
+         -0.6782682091832445], [1.351227727719687, -0.6538545081853449], [
+             1.365347842462401, -0.6294757302167044
+         ], [1.3789389411433586,
+             -0.6051419971134862], [1.391997656782351, -0.5808623313043475], [
+                 1.4045215487801634,
+                 -0.5566446698699925
+             ], [1.4165090423718034, -0.5324958842910054
+                 ], [1.4279593646268474, -0.5084218049460658
+                     ], [1.4388724778869602, -0.4844272494383845], [
+                         1.449249011452494, -0.460516053858597
+                     ], [1.4590901922431447, -0.4366911061340692
+                         ], [1.468397775065033, -0.4129543806643518
+                             ], [1.4771739730209745, -0.38930697349737264
+                                 ], [1.485421388504271, -0.36574913735813025],
+    [1.4931429451209484,
+     -0.3422803158986589], [1.5003418207921726, -0.3188991765927855], [
+         1.5070213821985838,
+         -0.2956036417497373
+     ], [1.513185120641734,
+         -0.2723909171654731], [1.5188365893149296, -0.24925751796822435], [
+             1.5239793418959948,
+             -0.22619929124396096
+         ], [1.5286168722972349,
+             -0.2032114350471563], [1.5327525553319497, -0.1802885134112242], [
+                 1.5363895879810432,
+                 -0.15742446697009113
+             ], [1.5395309308654115, -0.1346126187862511
+                 ], [1.5421792494481048, -0.11184567494963411
+                     ], [1.5443368544016174, -0.0891157194637005
+                         ], [1.5460056404769755, -0.06641420286771664
+                             ], [1.5471870230984175, -0.043731923953761714],
+    [1.547881871775246,
+     -0.021059003819238205], [1.5480904392645911, 0.0016151486553661097], [
+         1.547812285227133, 0.024301881005710235
+     ], [1.5470461928818935, 0.047013352405288866
+         ], [1.5457900768723736,
+             0.06976260156314103], [1.5440408801865422, 0.09256362934244797], [
+                 1.5417944575035705,
+                 0.11543149864415554
+             ], [1.5390454417383017, 0.13838245474060726
+                 ], [1.5357870897759938, 0.16143407007284788
+                     ], [1.5320111023738603, 0.18460541860588778
+                         ], [1.5277074118667517, 0.20791728625864334
+                             ], [1.5228639295308124, 0.23139242581719505
+                                 ], [1.5174662420569858, 0.25505586728497265],
+    [1.5114972433117844,
+     0.27893529808946027], [1.5049366830391921, 0.30306153234932315], [
+         1.4977606078174102,
+         0.32746909510770755
+     ], [1.4899406605544634,
+         0.35219695697813347], [1.4814431917184283, 0.37728946847450484], [
+             1.4722281161523716,
+             0.40279756372788583
+         ], [1.4622474200998372,
+             0.4287803341537376], [1.4514431778273647, 0.45530712040457033], [
+                 1.4397448652396179,
+                 0.48246034696312606
+             ], [1.427065639662639, 0.5103394485717625
+                 ], [1.4132970536897833, 0.5390664502570618
+                     ], [1.3983013135749314, 0.5687941401756967
+                         ], [1.3818995257862978, 0.5997184788509978
+                             ], [1.3638530549860057, 0.6320982830132342
+                                 ], [1.3438323057823602, 0.6662881915757862],
+    [1.3213606855701236,
+     0.7027978462604986], [1.2957042956404132, 0.7424084023365868], [
+         1.2656247456808565,
+         0.786433646353686
+     ], [1.2287034601894442,
+         0.8374338794107618], [1.1786904071656892, 0.902017503822168], [
+             1.0497616572686415,
+             1.0497616572686426
+         ], [0.9097522267255194,
+             1.1864251300690412], [0.8484431816837388, 1.2397127624624213], [
+                 0.8000049977037023,
+                 1.2791960970308718
+             ], [0.7581818853313602, 1.3114777786351866
+                 ], [0.7205493452982701, 1.3391121846078957
+                     ], [0.6882849734317291, 1.3617105524217008
+                         ], [0.6882849734317291, 2.356194490192345
+                             ], [1.3376784164442164, 2.356194490192345
+                                 ], [1.3516056511178856, 2.3360528189227487],
+    [1.3708660530144583,
+     2.308792458458483], [1.3913233553973305, 2.2804469909122105], [
+         1.4131687110627962,
+         2.2508089759388485
+     ], [1.436656614785,
+         2.219604401551449], [1.4621380338543308, 2.18645769238238], [
+             1.4901198983671067,
+             2.1508288797419346
+         ], [1.5213822452925796, 2.111889875368928
+             ], [1.5572408030205178, 2.0682471032649676
+                 ], [1.6002650085871786, 2.0171829478543404
+                     ], [1.657096323745671, 1.9516778011159774
+                         ], [1.7977393629734166, 1.7977393629734166
+                             ], [1.8577014383575772, 1.7364646168980775
+                                 ], [1.8577014383575772, -1.7353804562372057])
 
 # Test data set in terms of (theta0, theta1) of a motion from Parker.
 # This is an example box lift profile.
 
-path_with_accelerations = [(1.3583511559969876, 0.99753029519739866, 0.63708920330895369, -0.77079007974101643, -0.21483375398380378, -0.17756921128311187), (1.4037744780290744, 0.94141413786797179, 0.62102298265172207, -0.78379235452915641, -0.23084099683729808, -0.18290283743090688), (1.4401396868545593, 0.89467165253531666, 0.606891738771278, -0.79478451004733031, -0.24685749210031543, -0.18849894149927623), (1.4710837705397868, 0.85345617877170399, 0.59376006111610313, -0.80464215016577489, -0.26311393698286972, -0.19415687958799988), (1.4982967960378542, 0.81597755723156562, 0.58119389263733834, -0.81376511301545618, -0.27984413029527577, -0.19986598102433009), (1.5227269571172215, 0.78122893136122973, 0.56894801707769227, -0.82237348805962973, -0.29704891442706877, -0.20550961547441812), (1.5449685847030388, 0.74857685672553398, 0.55686625320351735, -0.83060217676278458, -0.31482379304256025, -0.21106978462024978), (1.5654226738527495, 0.71759174238125301, 0.54484048864732038, -0.83853970802255351, -0.33321287621660045, -0.21650513557965106), (1.5843743655705165, 0.68796601612512265, 0.53279106733541426, -0.84624681894089859, -0.35226023809265267, -0.22178093106252236), (1.6020345343012865, 0.65947020123078393, 0.52065635488289785, -0.85376633812774205, -0.37188463022164508, -0.22678852570730737), (1.6185639244576269, 0.63192742228662813, 0.50838674358661662, -0.86112886314731996, -0.39227952043050979, -0.23159139751917215), (1.6340880277208036, 0.60519768777296334, 0.49594100637973321, -0.868356216187261, -0.41330592927121557, -0.23605025316897368), (1.6487067026409843, 0.57916772325998889, 0.48328397846038601, -0.87546364639743945, -0.43479093110952094, -0.24001893904573279), (1.6625006419641934, 0.55374413183503834, 0.47038503527185527, -0.88246128447218319, -0.45715591769925784, -0.2436817242766752), (1.6755358637715485, 0.52884863988931285, 0.45721707379330323, -0.88935513009814537, -0.48005035022840359, -0.24679412703629519), (1.6878669166029825, 0.50441469954774187, 0.44375581873319114, -0.89614774079971626, -0.50380737957408728, -0.24947659419788909), (1.6995392207123023, 0.48038500205371104, 0.42997935653089725, -0.90283871923908732, -0.52783372117172589, -0.25138272577381626), (1.7105908129416818, 0.45670961972392227, 0.41586782149756141, -0.90942506840468851, -0.55267961927927023, -0.25273346631273114), (1.7210536699519405, 0.43334459201815007, 0.40140320353386438, -0.91590145113584731, -0.57770009966870806, -0.25318354738409404), (1.7309547270317198, 0.41025083198931545, 0.38656923494570777, -0.92226038979969771, -0.60317645612010684, -0.25282447346857195), (1.7403166729890138, 0.38739326814551689, 0.37135135004406289, -0.92849242044318914, -0.62907036438628838, -0.25159784841893079), (1.7491585775728877, 0.36474016215239319, 0.35573669259875446, -0.93458622156486959, -0.6551663269414254, -0.24938020059543903), (1.7574963917487576, 0.34226255982896742, 0.33971416817223166, -0.94052872574050006, -0.68164592193873064, -0.24620769226541636), (1.7653433501187576, 0.31993384454025714, 0.32327453032164366, -0.94630522456833177, -0.70757451300020135, -0.24172061458903144), (1.7727102970907476, 0.29772937021195395, 0.30641049133414705, -0.95189947515500117, -0.73378453575677771, -0.23620138680600353), (1.7796059529520747, 0.27562615695138953, 0.28911685939500692, -0.95729381154041093, -0.75990897245769828, -0.22950448838605128), (1.7860371320859008, 0.25360263640618674, 0.27139068895919466, -0.9624692690918778, -0.78523056852177775, -0.22141489322699751), (1.7920089227108962, 0.23163843702152076, 0.25323144050682866, -0.96740572540110403, -0.81025448009679157, -0.21209581939563796), (1.7975248354162883, 0.20971420159976159, 0.2346411508301583, -0.97208205946674009, -0.83408589705170777, -0.20133249883457041), (1.8025869261905516, 0.187811431247703, 0.21562459548465329, -0.97647633551565383, -0.85752536967991499, -0.18935884543741083), (1.8071958984562269, 0.16591235107247332, 0.19618945367704119, -0.98056600913243175, -0.8793458743889202, -0.17593847769244828), (1.8113511877225346, 0.14399979396679621, 0.17634645105214913, -0.98432816133711831, -0.89983233183814704, -0.16120962647016274), (1.8150510317784807, 0.12205709958524935, 0.15610948821796866, -0.98773975706575867, -0.91852354093170219, -0.14517104001739675), (1.8182925288197413, 0.10006802621144681, 0.13549574094137548, -0.99077792879471627, -0.93602333307674279, -0.12800867856143744), (1.821071685494891, 0.078016673692446373, 0.11452572935697461, -0.99342028231522084, -0.95112385045254688, -0.10965064890920556), (1.8233834565427998, 0.055887416001137141, 0.093223341590826445, -0.99564522224667962, -0.96368362602021329, -0.090231797498176183), (1.8252217774528923, 0.03366484230234558, 0.071615815013953282, -0.99743229095507402, -0.97395276760288163, -0.069931055757802438), (1.8265795913984428, 0.011333705660752967, 0.049733665943173709, -0.99876251555204698, -0.98177893706111263, -0.048889055531294821), (1.8274488715589303, -0.011121121248675203, 0.02761056315188902, -0.99961875572762016, -0.98689335559342106, -0.027260179511112551), (1.8278206398523622, -0.033714683874300592, 0.0052831412013899706, -0.99998604411213976, -0.98885489268650051, -0.0052254487261787384), (1.8276849830361703, -0.056461977241467148, -0.017209244112191453, -0.99985190999321838, -0.98812791978941406, 0.017006330622021722), (1.8270310671011307, -0.079377971247817633, -0.039824819694542858, -0.99920667718760625, -0.98450390283226485, 0.039237693528591036), (1.8258471508733223, -0.10247762977900209, -0.062519753866286318, -0.99804372668560926, -0.97719363903338485, 0.061212532653782022), (1.8241205997516765, -0.12577592348843702, -0.085248569812080663, -0.99635971483445407, -0.9676884196650537, 0.08279433361404552), (1.8218379005412455, -0.14928783595476772, -0.10796459505798382, -0.99415473957224865, -0.95496997071328249, 0.103708042997466), (1.8189846783932371, -0.17302836280361969, -0.13062045461685196, -0.99143244693508337, -0.93935720014890423, 0.12375848445745616), (1.8155457169306066, -0.19701250325490266, -0.15316858191907523, -0.98820007362522466, -0.92129127811992539, 0.14279680293047756), (1.8115049827211411, -0.22125524343262917, -0.17556174489393769, -0.98446842190585071, -0.90059657633599033, 0.16060369204333666), (1.8068456553568204, -0.24577153065112339, -0.19775357131740673, -0.98025176614541798, -0.87754964758424281, 0.17703366424976713), (1.8015501645066898, -0.27057623777063011, -0.21969906367786421, -0.97556769186923664, -0.85256620853260556, 0.19199796793136439), (1.795600235427889, -0.29568411659857541, -0.24135508622397667, -0.97043687190554384, -0.82563764988942245, 0.20534143268497296), (1.7889769445422168, -0.32110973920321939, -0.26268082100774776, -0.96488278369690872, -0.79733324597202848, 0.2170659935022537), (1.7816607868090406, -0.34686742590848585, -0.2836381738799823, -0.95893137727265387, -0.76782873106500538, 0.22711183686998365), (1.7736317567432636, -0.37297115865829411, -0.30419213280652752, -0.95261070030659223, -0.73738784843863214, 0.23546528518662449), (1.7648694450316516, -0.39943447838330554, -0.32431106313855179, -0.94595049253433039, -0.70610753433104578, 0.2420821140887684), (1.7553531527820141, -0.42627036498226889, -0.34396694344403017, -0.93898175797923322, -0.67467492193224388, 0.24714550708779132), (1.7450620254853675, -0.45349109855564618, -0.36313553561943973, -0.93173632684916963, -0.64280482438985487, 0.25052642309609846), (1.7339752087663065, -0.48110810061485765, -0.38179649614837224, -0.92424641493966653, -0.61125522193929116, 0.2525024014306243), (1.7220720279237476, -0.50913175415228962, -0.39993342129901005, -0.91654419343972093, -0.58016017720123902, 0.25315181203742848), (1.7093321931028456, -0.53757120171300954, -0.41753384214477446, -0.90866137293483673, -0.54964898595174738, 0.25256545404213349), (1.6957360316664474, -0.56643412097840984, -0.43458916639025197, -0.9006288116955985, -0.51972307699036913, 0.25078642241782151), (1.6812647489267576, -0.59572647787451594, -0.45109457862962754, -0.89247615157546845, -0.49088885529812037, 0.24811508685641395), (1.6659007178300929, -0.62545225787260295, -0.46704890024189227, -0.88423148823305242, -0.46296888429931843, 0.24453849915066983), (1.6496277974361373, -0.65561317697342414, -0.48245442463761296, -0.87592107415428122, -0.43630053035835431, 0.24031239163872659), (1.6324316790779632, -0.68620837486997288, -0.49731672574028513, -0.86756906024763358, -0.41095471910587472, 0.23557116495526012), (1.6143002579172752, -0.7172340939700278, -0.51164445121259294, -0.85919727393850864, -0.38675403181117768, 0.23030820687574571), (1.5952240262184549, -0.748683349319481, -0.52544910775441078, -0.85082503204836046, -0.36408233991880601, 0.22484810685703166), (1.5751964830679697, -0.780545595975073, -0.53874483689504815, -0.84246899095392713, -0.34276938388236106, 0.21919491474821423), (1.5542145534957212, -0.81280640198469889, -0.55154819264623711, -0.83414302801658013, -0.32291038254364612, 0.21351295685970414), (1.5322790080695103, -0.84544713677503402, -0.56387791937564813, -0.82585815491559456, -0.30444498192901781, 0.20786805829508584), (1.509394872119298, -0.87844468632349793, -0.57575473429345714, -0.81762245929198296, -0.28737482236853346, 0.20236376870812761), (1.4855718119204786, -0.91177120788178356, -0.58720111439137757, -0.8094410733694728, -0.27173204177162957, 0.1971250636942595), (1.4608244835702884, -0.94539393807497152, -0.59824108887263983, -0.80131616705547515, -0.25741396251510884, 0.19217806498346204), (1.4351728290976418, -0.97927506876108006, -0.60890003734400222, -0.79324696313473042, -0.24447483076597848, 0.18765975242966434), (1.4086423037364302, -1.0133717049339268, -0.61920449072176709, -0.78522977444184905, -0.2328103488745005, 0.18358578028961331), (1.3812640184459468, -1.047635918033063, -0.62918193604047223, -0.77725805969469564, -0.22240924347971955, 0.18003762666126619), (1.3530747828369902, -1.0820149061676485, -0.63886062143319422, -0.76932249829443622, -0.21321450222713362, 0.17705731568712871), (1.324117035769417, -1.1164512699023377, -0.64826936036845306, -0.76141108240389876, -0.20523063810290823, 0.17473421668463424), (1.294438654066159, -1.1508834084076087, -0.6574373330314689, -0.75350922564788103, -0.19832793065660315, 0.17304104866571926), (1.2640926339871807, -1.1852460360561055, -0.66639388399919663, -0.74559988691553936, -0.1925723524639939, 0.17211494490833781), (1.2331366451651338, -1.2194708141674491, -0.67516831336894412, -0.73766370970960415, -0.18786875722059337, 0.17195217715824379), (1.2016324623545263, -1.2534870868845998, -0.68378966242702388, -0.72967917440333774, -0.18419866624510314, 0.17261421082264014), (1.1696452862242701, -1.2872227045046678, -0.6922864921531301, -0.72162276348679166, -0.18150423850780073, 0.17412530594411615), (1.1372429700984137, -1.3206049124326309, -0.70068665547523046, -0.71346913797229905, -0.17977253485932801, 0.17655152952198189), (1.104495174566031, -1.3535612797241663, -0.70901706362016093, -0.70519132403585671, -0.17898758721428376, 0.17995840096834861), (1.0714724758096574, -1.3860206383273435, -0.71730344800943324, -0.69676090839955884, -0.17907637813730631, 0.18435585053172451), (1.0382454559924987, -1.4179140029119093, -0.72557011960647111, -0.68814824095848082, -0.18003314612431659, 0.18982321746977993), (1.0048838048727911, -1.4491754417344611, -0.73383972725116353, -0.67932264404179699, -0.18184420373071258, 0.19643734513631667), (0.97145546090878643, -1.4797428713062153, -0.74213301743428883, -0.67025262732336799, -0.18446052990619061, 0.20424250843873848)]
+path_with_accelerations = [
+    (1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
+     -0.77079007974101643, -0.21483375398380378, -0.17756921128311187),
+    (1.4037744780290744, 0.94141413786797179, 0.62102298265172207,
+     -0.78379235452915641, -0.23084099683729808, -0.18290283743090688),
+    (1.4401396868545593, 0.89467165253531666, 0.606891738771278,
+     -0.79478451004733031, -0.24685749210031543, -0.18849894149927623),
+    (1.4710837705397868, 0.85345617877170399, 0.59376006111610313,
+     -0.80464215016577489, -0.26311393698286972, -0.19415687958799988),
+    (1.4982967960378542, 0.81597755723156562, 0.58119389263733834,
+     -0.81376511301545618, -0.27984413029527577, -0.19986598102433009),
+    (1.5227269571172215, 0.78122893136122973, 0.56894801707769227,
+     -0.82237348805962973, -0.29704891442706877, -0.20550961547441812),
+    (1.5449685847030388, 0.74857685672553398, 0.55686625320351735,
+     -0.83060217676278458, -0.31482379304256025, -0.21106978462024978),
+    (1.5654226738527495, 0.71759174238125301, 0.54484048864732038,
+     -0.83853970802255351, -0.33321287621660045, -0.21650513557965106),
+    (1.5843743655705165, 0.68796601612512265, 0.53279106733541426,
+     -0.84624681894089859, -0.35226023809265267, -0.22178093106252236),
+    (1.6020345343012865, 0.65947020123078393, 0.52065635488289785,
+     -0.85376633812774205, -0.37188463022164508, -0.22678852570730737),
+    (1.6185639244576269, 0.63192742228662813, 0.50838674358661662,
+     -0.86112886314731996, -0.39227952043050979, -0.23159139751917215),
+    (1.6340880277208036, 0.60519768777296334, 0.49594100637973321,
+     -0.868356216187261, -0.41330592927121557, -0.23605025316897368),
+    (1.6487067026409843, 0.57916772325998889, 0.48328397846038601,
+     -0.87546364639743945, -0.43479093110952094, -0.24001893904573279),
+    (1.6625006419641934, 0.55374413183503834, 0.47038503527185527,
+     -0.88246128447218319, -0.45715591769925784, -0.2436817242766752),
+    (1.6755358637715485, 0.52884863988931285, 0.45721707379330323,
+     -0.88935513009814537, -0.48005035022840359, -0.24679412703629519),
+    (1.6878669166029825, 0.50441469954774187, 0.44375581873319114,
+     -0.89614774079971626, -0.50380737957408728, -0.24947659419788909),
+    (1.6995392207123023, 0.48038500205371104, 0.42997935653089725,
+     -0.90283871923908732, -0.52783372117172589, -0.25138272577381626),
+    (1.7105908129416818, 0.45670961972392227, 0.41586782149756141,
+     -0.90942506840468851, -0.55267961927927023, -0.25273346631273114),
+    (1.7210536699519405, 0.43334459201815007, 0.40140320353386438,
+     -0.91590145113584731, -0.57770009966870806, -0.25318354738409404),
+    (1.7309547270317198, 0.41025083198931545, 0.38656923494570777,
+     -0.92226038979969771, -0.60317645612010684, -0.25282447346857195),
+    (1.7403166729890138, 0.38739326814551689, 0.37135135004406289,
+     -0.92849242044318914, -0.62907036438628838, -0.25159784841893079),
+    (1.7491585775728877, 0.36474016215239319, 0.35573669259875446,
+     -0.93458622156486959, -0.6551663269414254, -0.24938020059543903),
+    (1.7574963917487576, 0.34226255982896742, 0.33971416817223166,
+     -0.94052872574050006, -0.68164592193873064, -0.24620769226541636),
+    (1.7653433501187576, 0.31993384454025714, 0.32327453032164366,
+     -0.94630522456833177, -0.70757451300020135, -0.24172061458903144),
+    (1.7727102970907476, 0.29772937021195395, 0.30641049133414705,
+     -0.95189947515500117, -0.73378453575677771, -0.23620138680600353),
+    (1.7796059529520747, 0.27562615695138953, 0.28911685939500692,
+     -0.95729381154041093, -0.75990897245769828, -0.22950448838605128),
+    (1.7860371320859008, 0.25360263640618674, 0.27139068895919466,
+     -0.9624692690918778, -0.78523056852177775, -0.22141489322699751),
+    (1.7920089227108962, 0.23163843702152076, 0.25323144050682866,
+     -0.96740572540110403, -0.81025448009679157, -0.21209581939563796),
+    (1.7975248354162883, 0.20971420159976159, 0.2346411508301583,
+     -0.97208205946674009, -0.83408589705170777, -0.20133249883457041),
+    (1.8025869261905516, 0.187811431247703, 0.21562459548465329,
+     -0.97647633551565383, -0.85752536967991499, -0.18935884543741083),
+    (1.8071958984562269, 0.16591235107247332, 0.19618945367704119,
+     -0.98056600913243175, -0.8793458743889202, -0.17593847769244828),
+    (1.8113511877225346, 0.14399979396679621, 0.17634645105214913,
+     -0.98432816133711831, -0.89983233183814704, -0.16120962647016274),
+    (1.8150510317784807, 0.12205709958524935, 0.15610948821796866,
+     -0.98773975706575867, -0.91852354093170219, -0.14517104001739675),
+    (1.8182925288197413, 0.10006802621144681, 0.13549574094137548,
+     -0.99077792879471627, -0.93602333307674279, -0.12800867856143744),
+    (1.821071685494891, 0.078016673692446373, 0.11452572935697461,
+     -0.99342028231522084, -0.95112385045254688, -0.10965064890920556),
+    (1.8233834565427998, 0.055887416001137141, 0.093223341590826445,
+     -0.99564522224667962, -0.96368362602021329, -0.090231797498176183),
+    (1.8252217774528923, 0.03366484230234558, 0.071615815013953282,
+     -0.99743229095507402, -0.97395276760288163, -0.069931055757802438),
+    (1.8265795913984428, 0.011333705660752967, 0.049733665943173709,
+     -0.99876251555204698, -0.98177893706111263, -0.048889055531294821),
+    (1.8274488715589303, -0.011121121248675203, 0.02761056315188902,
+     -0.99961875572762016, -0.98689335559342106, -0.027260179511112551),
+    (1.8278206398523622, -0.033714683874300592, 0.0052831412013899706,
+     -0.99998604411213976, -0.98885489268650051, -0.0052254487261787384),
+    (1.8276849830361703, -0.056461977241467148, -0.017209244112191453,
+     -0.99985190999321838, -0.98812791978941406, 0.017006330622021722),
+    (1.8270310671011307, -0.079377971247817633, -0.039824819694542858,
+     -0.99920667718760625, -0.98450390283226485, 0.039237693528591036),
+    (1.8258471508733223, -0.10247762977900209, -0.062519753866286318,
+     -0.99804372668560926, -0.97719363903338485, 0.061212532653782022),
+    (1.8241205997516765, -0.12577592348843702, -0.085248569812080663,
+     -0.99635971483445407, -0.9676884196650537, 0.08279433361404552),
+    (1.8218379005412455, -0.14928783595476772, -0.10796459505798382,
+     -0.99415473957224865, -0.95496997071328249, 0.103708042997466),
+    (1.8189846783932371, -0.17302836280361969, -0.13062045461685196,
+     -0.99143244693508337, -0.93935720014890423, 0.12375848445745616),
+    (1.8155457169306066, -0.19701250325490266, -0.15316858191907523,
+     -0.98820007362522466, -0.92129127811992539, 0.14279680293047756),
+    (1.8115049827211411, -0.22125524343262917, -0.17556174489393769,
+     -0.98446842190585071, -0.90059657633599033, 0.16060369204333666),
+    (1.8068456553568204, -0.24577153065112339, -0.19775357131740673,
+     -0.98025176614541798, -0.87754964758424281, 0.17703366424976713),
+    (1.8015501645066898, -0.27057623777063011, -0.21969906367786421,
+     -0.97556769186923664, -0.85256620853260556, 0.19199796793136439),
+    (1.795600235427889, -0.29568411659857541, -0.24135508622397667,
+     -0.97043687190554384, -0.82563764988942245, 0.20534143268497296),
+    (1.7889769445422168, -0.32110973920321939, -0.26268082100774776,
+     -0.96488278369690872, -0.79733324597202848, 0.2170659935022537),
+    (1.7816607868090406, -0.34686742590848585, -0.2836381738799823,
+     -0.95893137727265387, -0.76782873106500538, 0.22711183686998365),
+    (1.7736317567432636, -0.37297115865829411, -0.30419213280652752,
+     -0.95261070030659223, -0.73738784843863214, 0.23546528518662449),
+    (1.7648694450316516, -0.39943447838330554, -0.32431106313855179,
+     -0.94595049253433039, -0.70610753433104578, 0.2420821140887684),
+    (1.7553531527820141, -0.42627036498226889, -0.34396694344403017,
+     -0.93898175797923322, -0.67467492193224388, 0.24714550708779132),
+    (1.7450620254853675, -0.45349109855564618, -0.36313553561943973,
+     -0.93173632684916963, -0.64280482438985487, 0.25052642309609846),
+    (1.7339752087663065, -0.48110810061485765, -0.38179649614837224,
+     -0.92424641493966653, -0.61125522193929116, 0.2525024014306243),
+    (1.7220720279237476, -0.50913175415228962, -0.39993342129901005,
+     -0.91654419343972093, -0.58016017720123902, 0.25315181203742848),
+    (1.7093321931028456, -0.53757120171300954, -0.41753384214477446,
+     -0.90866137293483673, -0.54964898595174738, 0.25256545404213349),
+    (1.6957360316664474, -0.56643412097840984, -0.43458916639025197,
+     -0.9006288116955985, -0.51972307699036913, 0.25078642241782151),
+    (1.6812647489267576, -0.59572647787451594, -0.45109457862962754,
+     -0.89247615157546845, -0.49088885529812037, 0.24811508685641395),
+    (1.6659007178300929, -0.62545225787260295, -0.46704890024189227,
+     -0.88423148823305242, -0.46296888429931843, 0.24453849915066983),
+    (1.6496277974361373, -0.65561317697342414, -0.48245442463761296,
+     -0.87592107415428122, -0.43630053035835431, 0.24031239163872659),
+    (1.6324316790779632, -0.68620837486997288, -0.49731672574028513,
+     -0.86756906024763358, -0.41095471910587472, 0.23557116495526012),
+    (1.6143002579172752, -0.7172340939700278, -0.51164445121259294,
+     -0.85919727393850864, -0.38675403181117768, 0.23030820687574571),
+    (1.5952240262184549, -0.748683349319481, -0.52544910775441078,
+     -0.85082503204836046, -0.36408233991880601, 0.22484810685703166),
+    (1.5751964830679697, -0.780545595975073, -0.53874483689504815,
+     -0.84246899095392713, -0.34276938388236106, 0.21919491474821423),
+    (1.5542145534957212, -0.81280640198469889, -0.55154819264623711,
+     -0.83414302801658013, -0.32291038254364612, 0.21351295685970414),
+    (1.5322790080695103, -0.84544713677503402, -0.56387791937564813,
+     -0.82585815491559456, -0.30444498192901781, 0.20786805829508584),
+    (1.509394872119298, -0.87844468632349793, -0.57575473429345714,
+     -0.81762245929198296, -0.28737482236853346, 0.20236376870812761),
+    (1.4855718119204786, -0.91177120788178356, -0.58720111439137757,
+     -0.8094410733694728, -0.27173204177162957, 0.1971250636942595),
+    (1.4608244835702884, -0.94539393807497152, -0.59824108887263983,
+     -0.80131616705547515, -0.25741396251510884, 0.19217806498346204),
+    (1.4351728290976418, -0.97927506876108006, -0.60890003734400222,
+     -0.79324696313473042, -0.24447483076597848, 0.18765975242966434),
+    (1.4086423037364302, -1.0133717049339268, -0.61920449072176709,
+     -0.78522977444184905, -0.2328103488745005, 0.18358578028961331),
+    (1.3812640184459468, -1.047635918033063, -0.62918193604047223,
+     -0.77725805969469564, -0.22240924347971955, 0.18003762666126619),
+    (1.3530747828369902, -1.0820149061676485, -0.63886062143319422,
+     -0.76932249829443622, -0.21321450222713362, 0.17705731568712871),
+    (1.324117035769417, -1.1164512699023377, -0.64826936036845306,
+     -0.76141108240389876, -0.20523063810290823, 0.17473421668463424),
+    (1.294438654066159, -1.1508834084076087, -0.6574373330314689,
+     -0.75350922564788103, -0.19832793065660315, 0.17304104866571926),
+    (1.2640926339871807, -1.1852460360561055, -0.66639388399919663,
+     -0.74559988691553936, -0.1925723524639939, 0.17211494490833781),
+    (1.2331366451651338, -1.2194708141674491, -0.67516831336894412,
+     -0.73766370970960415, -0.18786875722059337, 0.17195217715824379),
+    (1.2016324623545263, -1.2534870868845998, -0.68378966242702388,
+     -0.72967917440333774, -0.18419866624510314, 0.17261421082264014),
+    (1.1696452862242701, -1.2872227045046678, -0.6922864921531301,
+     -0.72162276348679166, -0.18150423850780073, 0.17412530594411615),
+    (1.1372429700984137, -1.3206049124326309, -0.70068665547523046,
+     -0.71346913797229905, -0.17977253485932801, 0.17655152952198189),
+    (1.104495174566031, -1.3535612797241663, -0.70901706362016093,
+     -0.70519132403585671, -0.17898758721428376, 0.17995840096834861),
+    (1.0714724758096574, -1.3860206383273435, -0.71730344800943324,
+     -0.69676090839955884, -0.17907637813730631, 0.18435585053172451),
+    (1.0382454559924987, -1.4179140029119093, -0.72557011960647111,
+     -0.68814824095848082, -0.18003314612431659, 0.18982321746977993),
+    (1.0048838048727911, -1.4491754417344611, -0.73383972725116353,
+     -0.67932264404179699, -0.18184420373071258, 0.19643734513631667),
+    (0.97145546090878643, -1.4797428713062153, -0.74213301743428883,
+     -0.67025262732336799, -0.18446052990619061, 0.20424250843873848)
+]
diff --git a/y2018/control_loops/python/polydrivetrain.py b/y2018/control_loops/python/polydrivetrain.py
index c92e432..f406aa2 100644
--- a/y2018/control_loops/python/polydrivetrain.py
+++ b/y2018/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
 FLAGS = gflags.FLAGS
 
 try:
-  gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+    gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 except gflags.DuplicateFlagError:
-  pass
+    pass
+
 
 def main(argv):
-  if FLAGS.plot:
-    polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
-  elif len(argv) != 7:
-    glog.fatal('Expected .h file name and .cc file name')
-  else:
-    polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2018',
-                                       drivetrain.kDrivetrain)
+    if FLAGS.plot:
+        polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+    elif len(argv) != 7:
+        glog.fatal('Expected .h file name and .cc file name')
+    else:
+        polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+                                           'y2018', drivetrain.kDrivetrain)
+
 
 if __name__ == '__main__':
-  argv = FLAGS(sys.argv)
-  glog.init()
-  sys.exit(main(argv))
+    argv = FLAGS(sys.argv)
+    glog.init()
+    sys.exit(main(argv))
diff --git a/y2018/control_loops/python/polydrivetrain_test.py b/y2018/control_loops/python/polydrivetrain_test.py
index ccc3b77..3a1987a 100644
--- a/y2018/control_loops/python/polydrivetrain_test.py
+++ b/y2018/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
 
 
 class TestVelocityDrivetrain(unittest.TestCase):
-  def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
-    H = numpy.matrix([[1, 0],
-                      [-1, 0],
-                      [0, 1],
-                      [0, -1]])
-    K = numpy.matrix([[x1_max],
-                      [-x1_min],
-                      [x2_max],
-                      [-x2_min]])
-    return polytope.HPolytope(H, K)
 
-  def test_coerce_inside(self):
-    """Tests coercion when the point is inside the box."""
-    box = self.MakeBox(1, 2, 1, 2)
+    def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+        H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+        K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+        return polytope.HPolytope(H, K)
 
-    # x1 = x2
-    K = numpy.matrix([[1, -1]])
-    w = 0
+    def test_coerce_inside(self):
+        """Tests coercion when the point is inside the box."""
+        box = self.MakeBox(1, 2, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
-                                                 numpy.matrix([[1.5], [1.5]])),
-                       numpy.matrix([[1.5], [1.5]]))
+        # x1 = x2
+        K = numpy.matrix([[1, -1]])
+        w = 0
 
-  def test_coerce_outside_intersect(self):
-    """Tests coercion when the line intersects the box."""
-    box = self.MakeBox(1, 2, 1, 2)
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+            numpy.matrix([[1.5], [1.5]]))
 
-    # x1 = x2
-    K = numpy.matrix([[1, -1]])
-    w = 0
+    def test_coerce_outside_intersect(self):
+        """Tests coercion when the line intersects the box."""
+        box = self.MakeBox(1, 2, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
-                       numpy.matrix([[2.0], [2.0]]))
+        # x1 = x2
+        K = numpy.matrix([[1, -1]])
+        w = 0
 
-  def test_coerce_outside_no_intersect(self):
-    """Tests coercion when the line does not intersect the box."""
-    box = self.MakeBox(3, 4, 1, 2)
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+            numpy.matrix([[2.0], [2.0]]))
 
-    # x1 = x2
-    K = numpy.matrix([[1, -1]])
-    w = 0
+    def test_coerce_outside_no_intersect(self):
+        """Tests coercion when the line does not intersect the box."""
+        box = self.MakeBox(3, 4, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
-                       numpy.matrix([[3.0], [2.0]]))
+        # x1 = x2
+        K = numpy.matrix([[1, -1]])
+        w = 0
 
-  def test_coerce_middle_of_edge(self):
-    """Tests coercion when the line intersects the middle of an edge."""
-    box = self.MakeBox(0, 4, 1, 2)
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+            numpy.matrix([[3.0], [2.0]]))
 
-    # x1 = x2
-    K = numpy.matrix([[-1, 1]])
-    w = 0
+    def test_coerce_middle_of_edge(self):
+        """Tests coercion when the line intersects the middle of an edge."""
+        box = self.MakeBox(0, 4, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
-                       numpy.matrix([[2.0], [2.0]]))
+        # x1 = x2
+        K = numpy.matrix([[-1, 1]])
+        w = 0
 
-  def test_coerce_perpendicular_line(self):
-    """Tests coercion when the line does not intersect and is in quadrant 2."""
-    box = self.MakeBox(1, 2, 1, 2)
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+            numpy.matrix([[2.0], [2.0]]))
 
-    # x1 = -x2
-    K = numpy.matrix([[1, 1]])
-    w = 0
+    def test_coerce_perpendicular_line(self):
+        """Tests coercion when the line does not intersect and is in quadrant 2."""
+        box = self.MakeBox(1, 2, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
-                       numpy.matrix([[1.0], [1.0]]))
+        # x1 = -x2
+        K = numpy.matrix([[1, 1]])
+        w = 0
+
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+            numpy.matrix([[1.0], [1.0]]))
 
 
 if __name__ == '__main__':
-  unittest.main()
+    unittest.main()
diff --git a/y2018/control_loops/python/spline_generate.py b/y2018/control_loops/python/spline_generate.py
index cea3d7e..0dc5804 100644
--- a/y2018/control_loops/python/spline_generate.py
+++ b/y2018/control_loops/python/spline_generate.py
@@ -7,7 +7,7 @@
 # see yXXXX/control_loops/python/drivetrain.py for the current values
 
 kDrivetrain = drivetrain.DrivetrainParams(
-    J = 6.0,
+    J=6.0,
     mass=68.0,
     robot_radius=0.616 / 2.0,
     wheel_radius=0.127 / 2.0 * 120.0 / 118.0,
@@ -26,12 +26,15 @@
 
 drivetrain = drivetrain.Drivetrain(kDrivetrain)
 # set up coefficients for Hemrite basis function evaluation
-coeffs = np.array([[1, 0, 0, -10, 15, -6], [0, 1, 0, -6, 8, -3], [0, 0, 0.5, -1.5, 1.5, -0.5], [0, 0, 0, 0.5, -1, 0.5], [0, 0, 0, -4, 7, -3], [0, 0, 0, 10, -15, 6]])
+coeffs = np.array([[1, 0, 0, -10, 15, -6], [0, 1, 0, -6, 8, -3],
+                   [0, 0, 0.5, -1.5, 1.5, -0.5], [0, 0, 0, 0.5, -1, 0.5],
+                   [0, 0, 0, -4, 7, -3], [0, 0, 0, 10, -15, 6]])
 coeffs_prime = np.empty_like(coeffs)
 for ii in range(0, len(coeffs)):
     for jj in range(0, len(coeffs[ii]) - 1):
         coeffs_prime[ii][jj] = (jj + 1) * coeffs[ii][jj]
 
+
 def RungeKutta(f, x, dt):
     """4th order RungeKutta integration of F starting at X."""
     a = f(x)
@@ -41,24 +44,31 @@
 
     return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
 
+
 def normalize(v):
     norm = np.linalg.norm(v)
     return v / norm
 
+
 def theta(v):
     return np.arctan2(v[1], v[0])
 
+
 # evaluate Nth hermite basis function at t
 def nth_H(N, t):
-    return coeffs[N][0] + coeffs[N][1]*t + coeffs[N][2]*t**2 + coeffs[N][3]*t**3 + coeffs[N][4]*t**4 + coeffs[N][5]*t**5
+    return coeffs[N][0] + coeffs[N][1] * t + coeffs[N][2] * t**2 + coeffs[N][
+        3] * t**3 + coeffs[N][4] * t**4 + coeffs[N][5] * t**5
+
 
 def nth_H_prime(N, t):
-    return coeffs[N][0] + coeffs[N][1]*t + coeffs[N][2]*t**2 + coeffs[N][3]*t**3 + coeffs[N][4]*t**4 
+    return coeffs[N][0] + coeffs[N][1] * t + coeffs[N][2] * t**2 + coeffs[N][
+        3] * t**3 + coeffs[N][4] * t**4
+
 
 # class defining a quintic Hermite spline, with utilities for modification and plotting
 class Hermite_Spline:
     # init method given known parameters, ie savefile loading(if necessary)
-    def __init__(self, start, control1, control2, end, resolution = 200):
+    def __init__(self, start, control1, control2, end, resolution=200):
         self.start = start
         self.end = end
         self.control1 = control1
@@ -86,16 +96,18 @@
     # take paramters and compute coeffcicents for Hermite basis functions, to be called every time he change control points
     def compute_coefficients(self):
         self.coeffs = np.append(self.coeffs, np.array(self.start))
-        self.coeffs = np.append(self.coeffs, np.array(self.control1) - np.array(self.start))
-        self.coeffs = np.append(self.coeffs, [0,0])
-        self.coeffs = np.append(self.coeffs, [0,0])
-        self.coeffs = np.append(self.coeffs, np.array(self.end) - np.array(self.control2))
+        self.coeffs = np.append(self.coeffs,
+                                np.array(self.control1) - np.array(self.start))
+        self.coeffs = np.append(self.coeffs, [0, 0])
+        self.coeffs = np.append(self.coeffs, [0, 0])
+        self.coeffs = np.append(self.coeffs,
+                                np.array(self.end) - np.array(self.control2))
         self.coeffs = np.append(self.coeffs, np.array(self.end))
 
-        self.coeffs = np.reshape(self.coeffs, newshape = (6, 2))
+        self.coeffs = np.reshape(self.coeffs, newshape=(6, 2))
 
     # setters for control points, set coefficients
-    def set_positions(self, p1 = None, p2 = None):
+    def set_positions(self, p1=None, p2=None):
         if p1 != None:
             self.start = p1
         if p2 != None:
@@ -103,7 +115,7 @@
 
         self.compute_coefficients()
 
-    def set_controls(self, c1 = None, c2 = None):
+    def set_controls(self, c1=None, c2=None):
         if c1 != None:
             self.control1 = c1
         if c2 != None:
@@ -111,12 +123,12 @@
 
         self.compute_coefficients()
 
-    def set_velocities(self, v1 = None, v2 = None):
+    def set_velocities(self, v1=None, v2=None):
         if v1 != None:
             self.control1 = self.start + v1
         if v2 != None:
             self.control2 = self.end + v2
-        
+
         self.compute_coefficients()
 
     def get_smoothness(self):
@@ -125,14 +137,25 @@
 
     # given Basis functions and controls compute coordinate given t
     def spline_eval_hermite(self, t):
-        return np.array(self.coeffs[0]*nth_H(0, t) + self.coeffs[1]*nth_H(1, t)+ self.coeffs[2]*nth_H(2, t) + self.coeffs[3]*nth_H(3, t) + self.coeffs[4]* nth_H(4, t)+ self.coeffs[5]*nth_H(5, t))
-    
+        return np.array(self.coeffs[0] * nth_H(0, t) +
+                        self.coeffs[1] * nth_H(1, t) +
+                        self.coeffs[2] * nth_H(2, t) +
+                        self.coeffs[3] * nth_H(3, t) +
+                        self.coeffs[4] * nth_H(4, t) +
+                        self.coeffs[5] * nth_H(5, t))
+
     # given Basis functions and controls compute velocity given t
     def spline_eval_hermite_v(self, t):
-         return normalize(np.array(self.coeffs[0]*nth_H_prime(0, t) + self.coeffs[1]*nth_H_prime(1, t)+ self.coeffs[2]*nth_H_prime(2, t) + self.coeffs[3]*nth_H_prime(3, t) + self.coeffs[4]* nth_H_prime(4, t)+ self.coeffs[5]*nth_H_prime(5, t)))
+        return normalize(
+            np.array(self.coeffs[0] * nth_H_prime(0, t) +
+                     self.coeffs[1] * nth_H_prime(1, t) +
+                     self.coeffs[2] * nth_H_prime(2, t) +
+                     self.coeffs[3] * nth_H_prime(3, t) +
+                     self.coeffs[4] * nth_H_prime(4, t) +
+                     self.coeffs[5] * nth_H_prime(5, t)))
 
     # take coefficients and compute spline points/properties
-    def setup(self, resolution_multiplier = 10, dt = .000001):
+    def setup(self, resolution_multiplier=10, dt=.000001):
         points = []
         velocities = []
         accelerations = []
@@ -145,7 +168,7 @@
         distance = 0
 
         # iterate through interim points and compute pos_vectors, and at predefined points arc length,
-        # velocity, and acceleration vectors and store them at their associated index 
+        # velocity, and acceleration vectors and store them at their associated index
         for i in range(0, self.resolution * resolution_multiplier):
             t = i / (1.0 * self.resolution * resolution_multiplier)
 
@@ -157,7 +180,7 @@
 
             distance = current_s + distance
             # at important points compute important values and store
-            if i % resolution_multiplier  == 0:
+            if i % resolution_multiplier == 0:
                 s.append(distance)
                 points.append(current_point)
 
@@ -171,7 +194,8 @@
                 if np.linalg.norm(v) == 0:
                     curvature = 0
                 else:
-                    curvature = np.linalg.det(np.column_stack((v, a)) / (np.linalg.norm(v)**(3/2)))
+                    curvature = np.linalg.det(
+                        np.column_stack((v, a)) / (np.linalg.norm(v)**(3 / 2)))
 
                 velocities.append(v)
                 accelerations.append(a)
@@ -181,18 +205,17 @@
                     curvatures.append(0.0001)
                 else:
                     curvatures.append(curvature)
- 
+
             last_point = current_point
 
         self.arc_lengths = np.array(s)
-        self.points = np.reshape(points, newshape = (-1, 2))
-        self.velocities = np.reshape(velocities, newshape = (-1, 2))
-        self.accelerations = np.reshape(accelerations, newshape = (-1, 2))
+        self.points = np.reshape(points, newshape=(-1, 2))
+        self.velocities = np.reshape(velocities, newshape=(-1, 2))
+        self.accelerations = np.reshape(accelerations, newshape=(-1, 2))
         self.thetas = np.array(thetas)
         self.omegas = np.array(omegas)
         self.curvatures = np.array(curvatures)
 
-
     def plot_diagnostics(self):
         plt.figure("Spline")
         plt.title('Spline')
@@ -201,38 +224,48 @@
 
         plt.figure("Diagnostics")
 
-        plt.subplot(2, 2, 1)        
+        plt.subplot(2, 2, 1)
         plt.title('theta')
         plt.xlabel('arc_length')
         plt.ylabel('theta')
-        theta, = plt.plot(self.arc_lengths, self.thetas, label = 'theta')
-        plt.legend(handles = [theta])
+        theta, = plt.plot(self.arc_lengths, self.thetas, label='theta')
+        plt.legend(handles=[theta])
 
         plt.subplot(2, 2, 2)
         plt.title('omegas')
         plt.xlabel('arc_length')
         plt.ylabel('omega')
-        omega, = plt.plot(self.arc_lengths, self.omegas, label = 'omega')
-        plt.legend(handles = [omega])
+        omega, = plt.plot(self.arc_lengths, self.omegas, label='omega')
+        plt.legend(handles=[omega])
 
         plt.subplot(2, 2, 3)
         plt.title('Velocities')
         plt.xlabel('arc_length')
         plt.ylabel('velocity')
-        dxds, = plt.plot(self.arc_lengths, self.velocities[:, 0], label = 'dx/ds')
-        dyds, = plt.plot(self.arc_lengths, self.velocities[:, 1], label = 'dy/ds')
-        plt.legend(handles = [dxds, dyds])
+        dxds, = plt.plot(self.arc_lengths,
+                         self.velocities[:, 0],
+                         label='dx/ds')
+        dyds, = plt.plot(self.arc_lengths,
+                         self.velocities[:, 1],
+                         label='dy/ds')
+        plt.legend(handles=[dxds, dyds])
 
         plt.subplot(2, 2, 4)
         plt.title('Accelerations')
         plt.xlabel('arc_length')
         plt.ylabel('acceleration')
-        dx2ds2, = plt.plot(self.arc_lengths, self.accelerations[:, 0], label = 'd^2x/ds^2')
-        dy2ds2, = plt.plot(self.arc_lengths, self.accelerations[:, 1], label = 'd^2x/ds^2')
-        plt.legend(handles = [dx2ds2, dy2ds2])
+        dx2ds2, = plt.plot(self.arc_lengths,
+                           self.accelerations[:, 0],
+                           label='d^2x/ds^2')
+        dy2ds2, = plt.plot(self.arc_lengths,
+                           self.accelerations[:, 1],
+                           label='d^2x/ds^2')
+        plt.legend(handles=[dx2ds2, dy2ds2])
+
 
 # class defining a number of splines with convinience methods
 class Path:
+
     def __init__(self):
         self.splines = []
         self.knot_accels = []
@@ -270,7 +303,7 @@
         else:
             print("index %f out of bounds, no spline of that index" % i)
 
-    def join(self, first_priority = False):
+    def join(self, first_priority=False):
         for i in range(0, len(self.splines)):
             if first_priority & i != len(self.splines):
                 print("unfinished")
@@ -278,7 +311,12 @@
 
 # class which takes a Path object along with constraints and reparamterizes it with respect to time
 class Trajectory:
-    def __init__(self, path, max_angular_accel=3, max_voltage=11, max_normal_accel = .2):
+
+    def __init__(self,
+                 path,
+                 max_angular_accel=3,
+                 max_voltage=11,
+                 max_normal_accel=.2):
         self.path = path
         self.A = drivetrain.A_continuous
         self.B = drivetrain.B_continuous
@@ -291,43 +329,46 @@
 
         self.max_velocities_adhering_to_normal_accel = []
         self.max_velocities_adhering_to_voltage = []
-        self.path.splines[0].setup(resolution_multiplier = 100)
+        self.path.splines[0].setup(resolution_multiplier=100)
 
         self.set_max_v_adhering_to_normal_accel()
         self.max_voltageK_pass()
 
     def set_max_v_adhering_to_normal_accel(self):
         Ks = self.path.get_K()
-        accels = np.full_like(Ks, fill_value = self.max_normal_accel)
+        accels = np.full_like(Ks, fill_value=self.max_normal_accel)
         max_velocities = np.sqrt(np.abs(accels / Ks))
         self.max_velocities_adhering_to_normal_accel = max_velocities
 
     def max_voltageK_pass(self):
         max_ds_dt = []
         Ks = self.path.get_K()
-        turning_radii = np.full_like(Ks, fill_value = 1) / np.abs(Ks)
+        turning_radii = np.full_like(Ks, fill_value=1) / np.abs(Ks)
 
-    
-
-        # compute max steady-state velocity given voltage constraints  
+        # compute max steady-state velocity given voltage constraints
         for i in range(0, len(Ks)):
-            v_ratio = (turning_radii[i] + self.robot_radius) / (turning_radii[i] - self.robot_radius)
-            matrix = np.array([[self.A[1, 1], self.A[1, 3], self.B[1, 1]], [self.A[3, 1] - 1, self.A[3, 3], self.B[3, 1]], [-1, v_ratio, 0]])
-            sols = np.array([-1 * self.max_voltage * self.B[1, 0], -1 * self.max_voltage * self.B[3, 0], 0])
+            v_ratio = (turning_radii[i] + self.robot_radius) / (
+                turning_radii[i] - self.robot_radius)
+            matrix = np.array([[self.A[1, 1], self.A[1, 3], self.B[1, 1]],
+                               [self.A[3, 1] - 1, self.A[3, 3], self.B[3, 1]],
+                               [-1, v_ratio, 0]])
+            sols = np.array([
+                -1 * self.max_voltage * self.B[1, 0],
+                -1 * self.max_voltage * self.B[3, 0], 0
+            ])
             Vs = np.dot(np.linalg.inv(matrix), sols)
             max_ds_dt.append((Vs[0] + Vs[1]) / 2)
-            
+
         self.max_velocities_adhering_to_voltage = max_ds_dt
+
     # compute the maximum acceleration we can ask for given voltage and, ya know, staying on the path.
-
-
     '''
     These methods use the continuous form of our drivetrain state equation
     in order to compute the maximum acceleration which adheres to the path
     and voltage constraints, as well as any arbitary set of constraints
     on velocity as a function of arc_length
     '''
-    
+
     def forward_accel_pass(self):
         points = self.path.get_points()
         velocities = self.path.get_velocities()
@@ -335,27 +376,37 @@
         arc_lenghts = self.path.get_S()
 
         for i in range(0, len(points)):
-            Xn1 =   
-        
-    
+            #Xn1 =
+            pass
+
     def backward_accelaration_pass(self):
 
-        print("max backward accel pass")    
+        print("max backward accel pass")
 
-    
-    def plot_diagnostics(self, i = 0):
+    def plot_diagnostics(self, i=0):
 
         plt.figure('max velocity')
         plt.title('max_v_normal_accel')
         plt.xlabel('arc_length')
         plt.ylabel('max V')
-        max_v_normal = plt.plot(self.path.get_S(), self.max_velocities_adhering_to_normal_accel, label = 'ds/dt (normal)')#   , label = 'ds/dt')
-        curvature = plt.plot(self.path.get_S(), 1000 * np.abs(self.path.get_K()), label = 'K')
-        max_v_K_V = plt.plot(self.path.get_S(), self.max_velocities_adhering_to_voltage, label = 'ds/dt (voltage)')
-        plt.legend(handles = [max_v_normal[0], curvature[0], max_v_K_V[0]])
+        max_v_normal = plt.plot(self.path.get_S(),
+                                self.max_velocities_adhering_to_normal_accel,
+                                label='ds/dt (normal)')  #   , label = 'ds/dt')
+        curvature = plt.plot(self.path.get_S(),
+                             1000 * np.abs(self.path.get_K()),
+                             label='K')
+        max_v_K_V = plt.plot(self.path.get_S(),
+                             self.max_velocities_adhering_to_voltage,
+                             label='ds/dt (voltage)')
+        plt.legend(handles=[max_v_normal[0], curvature[0], max_v_K_V[0]])
+
 
 def main():
-    A = Hermite_Spline(np.array([0,0]), np.array([0,400]), np.array([200,300]), np.array([200,200]), resolution = 200)
+    A = Hermite_Spline(np.array([0, 0]),
+                       np.array([0, 400]),
+                       np.array([200, 300]),
+                       np.array([200, 200]),
+                       resolution=200)
     A.plot_diagnostics()
     path = Path()
     path.add_spline(A)
@@ -363,4 +414,5 @@
     trajectory.plot_diagnostics()
     plt.show()
 
+
 main()