diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index d708666..583d683 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -19,11 +19,12 @@ 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.5; // motor acceleration feedforward
+const float FF_ACCEL = 2.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.7; // proportional
-const float KI = 0.5; // integral
+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.07; // position integral
+const float KPI = 0.17; // position integral
+const float KRP = 0.07; // rotation proportional
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index de98ee2..2b8090c 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -52,7 +52,7 @@ void setup() {
     left_motor.write_rpm(0.0);
     delay(1);
     right_motor.write_rpm(0.0);
-    delay(999);
+    delay(2999);
     
     phase_start = (float)millis() / 1000.0;
     robot_state = FORWARD;
@@ -122,6 +122,10 @@ void loop() {
   Serial.print(pos_err_left);
   Serial.print(",er:");
   Serial.print(pos_err_right);
+  Serial.print(",il:");
+  Serial.print(left_iaccum);
+  Serial.print(",ir:");
+  Serial.print(right_iaccum);
   //Serial.print("ff:");
   //Serial.print(left_effort);
   //Serial.print(",time:");
@@ -153,6 +157,7 @@ void loop() {
     switch (robot_state) {
       case FORWARD:
         robot_state = TURN;
+				right_iaccum = left_iaccum = 8.0;
         phase_start = (float)millis() / 1000.0;
         break;
       case TURN:
diff --git a/robot_controller/trapezoidal.ino b/robot_controller/trapezoidal.ino
index a1ce35a..e81843f 100644
--- a/robot_controller/trapezoidal.ino
+++ b/robot_controller/trapezoidal.ino
@@ -11,6 +11,10 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
   float time_accelerating = max_vel / max_acc;
   float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
 
+	if (time < 0.0) {
+		return setpoint;
+	}
+
   if (2.0 * distance_while_accelerating > dist) {
     // triangular
     float peak_velocity_time = sqrt(dist/max_acc);