diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index a19bd32..594b71a 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -16,8 +16,6 @@ enum ROBOT_STATE {
   COMPLETE,
 };
 
-float CURRENT_POSITION = 0.0;
-float CURRENT_ROTATION = 0.0;
 struct Setpoint setpoint; 
 // start of current state
 float phase_start;
@@ -33,15 +31,6 @@ float time_p = 0.0;
 float left_home = 0.0;
 float right_home = 0.0;
 
-void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) {
-  if (rpm == 0) {rpm = 1;};
-  float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
-  motor->tune_vel_pid(kV, KP,KI,KD);
-  delay(1);
-  motor->write_rpm(rpm);
-  delay(1);
-}
-
 void setup() {
     // INIT SERIAL
     Serial.begin(115200);
@@ -58,16 +47,16 @@ void setup() {
     right_motor.tune_vel_pid(Kv, KP,KI,KD);
     delay(1);
 
-		delay(10);
+    delay(10);
     // runs to mend bad state
-		left_motor.home();
+    left_motor.home();
     delay(1);
     left_motor.write_angle(0.0);
     delay(1);
-		right_motor.home();
+    right_motor.home();
     delay(1);
     right_motor.write_angle(0.0);
-		delay(3000);
+    delay(3000);
     
     phase_start = (float)millis() / 1000.0;
     robot_state = FORWARD;
@@ -110,17 +99,9 @@ void loop() {
   float right_effort = pos_err_right * KPP + right_iaccum * KPI;
   float left_effort = pos_err_left * KPP + left_iaccum * KPI;
 
-  
-#define sgn(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
-  // boost on when slowing down
-  //if (setpoint.velocity < V_MIN && setpoint.acceleration < 0.0 && total_pos_error > 4.0) {
-  //  right_effort += (float )sgn(right_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
-  //  left_effort += (float )sgn(left_effort) * (setpoint.velocity - V_MIN) * V_KMIN;
-  //}
   float right_velocity = setpoint.velocity + right_effort;
   float left_velocity = (turnsig * setpoint.velocity) + left_effort;
   
-  // arbitrary feedforward with on-board velocity PID
 	if (setpoint.complete && robot_state == TURN) {
 		right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
 		left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
@@ -165,7 +146,7 @@ void loop() {
   //Serial.print(error / CMS_RPM);
   Serial.println("");
 
-  // move on if at setpoint TODO: give up after timeout
+  // move on if at setpoint or timeout
   if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 5.0)) {
     // rehome
     right_home += (setpoint.position / DEG_CM);
@@ -175,7 +156,7 @@ void loop() {
 
     switch (robot_state) {
       case FORWARD:
-        // turn tuning
+        // turn tuning (more aggressive)
         left_motor.tune_vel_pid(TURN_Kv, TURN_KP,TURN_KI,TURN_KD);
         delay(1);
         right_motor.tune_vel_pid(TURN_Kv,TURN_KP,TURN_KI,TURN_KD);
@@ -185,7 +166,7 @@ void loop() {
         phase_start = (float)millis() / 1000.0;
         break;
       case TURN:
-        // straight line tuning
+        // straight line tuning (less aggressive)
         left_motor.tune_vel_pid(Kv, KP,KI,KD);
         delay(1);
         right_motor.tune_vel_pid(Kv, KP,KI,KD);