diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 5dc1cb7..4cf6675 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -1,11 +1,9 @@
 #pragma once
 
-const float ACCEL_LIMIT = 5.0; // cm/s/s
-const float VEL_LIMIT = 16.0; // cm/s
-const float BACKLASH_RIGHT = 0.0; // degrees
-const float BACKLASH_LEFT = 0.0; // degrees
+const float ACCEL_LIMIT = 7.0; // cm/s/s
+const float VEL_LIMIT = 8.0; // cm/s
 const float WHEEL_DIAMETER = 6.61; // cm
-const float WHEEL_TO_WHEEL = 9.0; // cm
+const float WHEEL_TO_WHEEL = 8.6; // cm
 
 // centimeters per second to rpm
 const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
@@ -14,18 +12,14 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
 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 = 40.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 = 5.4; // motor acceleration feedforward
-const float FF_VEL = 0.9; // motor velocity feedforward
-const float FF_STAT = 18.4; // motor static friction
-const float KP = 0.00; // proportional
-const float KI = 0.0; // integral
-const float KD = 0.0; // derivative
-const float Kv = 1.0; // onboard velocity feedforward
+const float KP = 1.30; // proportional
+const float KI = 0.20; // integral
+const float KD = 0.01; // derivative
+const float Kv = 1.5; // onboard velocity feedforward
 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 801c322..51d55c0 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -50,19 +50,24 @@ void setup() {
     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);
-    left_motor.tune_vel_pid(Kv, KP,KI,KD);
-    left_motor.tune_vel_pid(Kv, KP,KI,KD);
-
-		//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);
+		right_motor.tune_pos_pid(0.4, 0.024, 0.008);
+    delay(1);
+    left_motor.tune_vel_pid(Kv, KP,KI,KD);
+    delay(1);
+    right_motor.tune_vel_pid(Kv, KP,KI,KD);
+    delay(1);
+
+		delay(10);
+    // runs to mend bad state
+		left_motor.home();
+    delay(1);
+    left_motor.write_angle(0.0);
+    delay(1);
+		right_motor.home();
+    delay(1);
+    right_motor.write_angle(0.0);
+		delay(3000);
     
     phase_start = (float)millis() / 1000.0;
     robot_state = FORWARD;
@@ -111,12 +116,14 @@ void loop() {
 		left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
 	} else {
     right_motor.write_rpm(right_velocity * CMS_RPM);
+    delay(1);
     left_motor.write_rpm(left_velocity * CMS_RPM);
+    delay(1);
 	}
 
   // send telemetry
-  int32_t rpm = left_motor.read_rpm();
-  int32_t pos = left_motor.read_angle();
+  int32_t rpm = right_motor.read_rpm();
+  int32_t pos = right_motor.read_angle();
   int32_t error = setpoint.velocity * CMS_RPM - rpm;
   Serial.print("el:");
   Serial.print(pos_err_left);
@@ -138,8 +145,10 @@ void loop() {
   Serial.print(rpm / CMS_RPM);
   Serial.print(",setvel:");
   Serial.print(setpoint.velocity);
-  Serial.print(",setacc:");
-  Serial.print(setpoint.acceleration);
+  Serial.print(",rvel:");
+  Serial.print(right_velocity);
+  //Serial.print(",setacc:");
+  //Serial.print(setpoint.acceleration);
   //Serial.print(",Err:");
   //Serial.print(error / CMS_RPM);
   Serial.println("");