diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index a8026fe..04bca67 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -17,9 +17,9 @@ const float FORWARD_DISTANCE = 30.0; // cm
 const float TURN_AMOUNT = 180.0; // degrees
 const float RETURN_DISTANCE = 100.0; // cm
 
-const float FF_ACCEL = 100.0; // motor acceleration feedforward
-const float FF_VEL = 0.9; // motor velocity feedforward
-const float FF_STAT = 100.0; // motor static friction
-const float KP = 3.7; // proportional
-const float KI = 0.3; // integral
+const float FF_ACCEL = 3.0; // motor acceleration feedforward
+const float FF_VEL = 3.9; // motor velocity feedforward
+const float FF_STAT = 2.4; // motor static friction
+const float KP = 3.0; // proportional
+const float KI = 0.0; // integral
 const float KD = 0.0; // derivative
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index 10501e5..546b051 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -41,9 +41,9 @@ void setup() {
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
     
     // TUNE VELOCITY PID
-    left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
-    delay(10);
-    right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
+    //left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
+    //delay(10);
+    //right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
     delay(10);
     //right_motor.set_direction(PIDDirection::DIRECT);
     
@@ -71,31 +71,34 @@ void loop() {
       break;
   }
 
-
   float feedforward = 
     setpoint.acceleration * CMS_RPM * FF_ACCEL + 
     setpoint.velocity * CMS_RPM * FF_VEL + 
-    FF_STAT;
+    ((setpoint.velocity > 0.0) ? FF_STAT : -FF_STAT);
 
-  left_motor.write_rpm_ff(setpoint.velocity * CMS_RPM, feedforward);
+  write_rpm_ff(&left_motor, setpoint.velocity * CMS_RPM, feedforward);
   delay(1);
-  right_motor.write_rpm_ff((robot_state == TURN ? velocity : -velocity) * CMS_RPM, feedforward);
+  //write_rpm_ff(&right_motor, (robot_state == TURN ? velocity : -velocity) * CMS_RPM, feedforward);
   delay(19);
 
   // READ MOTOR POSITION
-  int32_t rpm = right_motor.read_rpm();
-  int32_t pos = right_motor.read_angle();
+  int32_t rpm = left_motor.read_rpm();
+  int32_t pos = left_motor.read_angle();
   int32_t error = setpoint.velocity * CMS_RPM - rpm;
-  Serial.print("time:");
+  Serial.print("ff:");
+  Serial.print(feedforward);
+  Serial.print(",time:");
   Serial.print(time);
-  Serial.print(",dist goal:");
+  Serial.print(",distgoal:");
   Serial.print(setpoint.position);
   Serial.print(",dist:");
   Serial.print(pos * DEG_CM);
   Serial.print(",cms/s:");
   Serial.print(rpm / CMS_RPM);
-  Serial.print(",Setpoint:");
+  Serial.print(",setvel:");
   Serial.print(setpoint.velocity);
+  Serial.print(",setacc:");
+  Serial.print(setpoint.acceleration);
   Serial.print(",Err:");
   Serial.print(error / CMS_RPM);
   Serial.println("");