diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 55cbda2..5dc1cb7 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -22,9 +22,10 @@ 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 = 9.13; // proportional
+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 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 ea5a11f..801c322 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -49,8 +49,10 @@ 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_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();
@@ -103,23 +105,13 @@ void loop() {
   float right_velocity = setpoint.velocity + right_effort;
   float left_velocity = (turnsig * setpoint.velocity) + left_effort;
   
-  // calculate feedforward from motion profile and position PI data
-  float feedforward_right = 
-    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
-    right_velocity * CMS_RPM * FF_VEL + 
-    ((right_velocity > 0.0) ? FF_STAT : -FF_STAT);
-  float feedforward_left = 
-    setpoint.acceleration * CMS_RPM * FF_ACCEL + 
-    left_velocity * CMS_RPM * FF_VEL + 
-    ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
-
   // arbitrary feedforward with on-board velocity PID
 	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 {
-		write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
-		write_rpm_ff(&left_motor, left_velocity * CMS_RPM  , feedforward_left);
+    right_motor.write_rpm(right_velocity * CMS_RPM);
+    left_motor.write_rpm(left_velocity * CMS_RPM);
 	}
 
   // send telemetry