diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 583d683..c5351de 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -13,18 +13,18 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
 // degrees to centimeters
 const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
 
-const float FORWARD_DISTANCE = 73.0; // cm
-//const float FORWARD_DISTANCE = 10.0; // cm
+//const float FORWARD_DISTANCE = 73.0; // cm
+const float FORWARD_DISTANCE = 10.0; // cm
 const float TURN_AMOUNT = 146.0; // degrees
 const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
 const float RETURN_DISTANCE = 100.0; // cm
 
-const float FF_ACCEL = 2.4; // motor acceleration feedforward
+const float FF_ACCEL = 5.4; // motor acceleration feedforward
 const float FF_VEL = 0.9; // motor velocity feedforward
-const float FF_STAT = 16.4; // motor static friction
-const float KP = 2.9; // proportional
-const float KI = 0.8; // integral
-const float KD = 0.05; // derivative
-const float KPP = 0.25; // position proportional
-const float KPI = 0.17; // position integral
-const float KRP = 0.07; // rotation proportional
+const float FF_STAT = 18.4; // motor static friction
+const float KP = 9.13; // proportional
+const float KI = 0.0; // integral
+const float KD = 0.0; // derivative
+const float KPP = 0.0; // position proportional
+const float KPI = 0.0; // position integral
+const float KRP = 0.0; // rotation proportional
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index 2b8090c..ea5a11f 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -48,11 +48,19 @@ void setup() {
     while(!Serial);
 
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
+
+		//left_motor.tune_pos_pid(0.4, 0.024, 0.008);
+		//right_motor.tune_pos_pid(0.4, 0.024, 0.008);
+
+		//delay(1000);
+		//left_motor.home();
+		//left_motor.write_angle(180);
+		//delay(3000);
     
     left_motor.write_rpm(0.0);
     delay(1);
     right_motor.write_rpm(0.0);
-    delay(2999);
+    //delay(2999);
     
     phase_start = (float)millis() / 1000.0;
     robot_state = FORWARD;
@@ -106,7 +114,7 @@ void loop() {
     ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
 
   // arbitrary feedforward with on-board velocity PID
-	if (setpoint.complete) {
+	if (setpoint.complete && 0) {
 		right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
 		left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
 	} else {
@@ -134,12 +142,12 @@ void loop() {
   //Serial.print(setpoint.position);
   //Serial.print(",dist:");
   //Serial.print(pos * DEG_CM);
-  //Serial.print(",cms/s:");
-  //Serial.print(rpm / CMS_RPM);
-  //Serial.print(",setvel:");
-  //Serial.print(setpoint.velocity);
-  //Serial.print(",setacc:");
-  //Serial.print(setpoint.acceleration);
+  Serial.print(",vel:");
+  Serial.print(rpm / CMS_RPM);
+  Serial.print(",setvel:");
+  Serial.print(setpoint.velocity);
+  Serial.print(",setacc:");
+  Serial.print(setpoint.acceleration);
   //Serial.print(",Err:");
   //Serial.print(error / CMS_RPM);
   Serial.println("");
@@ -159,6 +167,11 @@ void loop() {
         robot_state = TURN;
 				right_iaccum = left_iaccum = 8.0;
         phase_start = (float)millis() / 1000.0;
+        // end
+        left_motor.write_rpm(0.0);
+        delay(1);
+        right_motor.write_rpm(0.0);
+        for (;;) {};
         break;
       case TURN:
         robot_state = RETURN;