diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index c2e6a10..60c34fb 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -1,15 +1,18 @@
 #pragma once
 
-const float ACCEL_LIMIT = 4.0; // cm/s/s
-const float VEL_LIMIT = 16.0; // cm/s
+const float ACCEL_LIMIT = 5.0; // cm/s/s
+const float VEL_LIMIT = 15.0; // cm/s
 const float BACKLASH_RIGHT = 0.0; // degrees
 const float BACKLASH_LEFT = 0.0; // degrees
 const float WHEEL_DIAMETER = 6.37; // cm
 const float WHEEL_TO_WHEEL = 0.0; // cm
 
-const float FORWARD_DISTANCE = 100.0; // cm
 // centimeters per second to rpm
 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 = 30.0; // cm
 const float TURN_AMOUNT = 180.0; // degrees
 const float RETURN_DISTANCE = 100.0; // cm
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index 8fe78a2..d638944 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -2,6 +2,7 @@
 #include <Wire.h>
 #include <smartmotor.h>
 #include <SMC_gains.h>
+#include <SMC_pid_directions.h>
 #include "trapezoidal.h"
 #include "constants.h"
 
@@ -32,9 +33,21 @@ void setup() {
 
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
     
-    // TUNE POSITION PID
-    left_motor.tune_vel_pid(9.0, 0.0,0.0,0.0); 
-    right_motor.tune_vel_pid(1.0, 0.65,0.060,0.065); 
+    // TUNE VELOCITY PID
+    left_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
+    delay(10);
+    right_motor.tune_vel_pid(0.0, 3.7,0.3,0.0); 
+    delay(10);
+    right_motor.set_direction(PIDDirection::DIRECT);
+    
+    uint16_t ratio = right_motor.get_gear_ratio();
+    Serial.print("ratio: ");
+    Serial.println(ratio);
+    ratio = left_motor.get_gear_ratio();
+    Serial.print("ratio: ");
+    Serial.println(ratio);
+
+
     delay(1000);
 
     // SET MOTOR POSITION
@@ -67,18 +80,26 @@ void loop() {
 
 
   left_motor.write_rpm(setpoint.velocity * CMS_RPM);
-  //right_motor.write_rpm(robot_state == TURN ? velocity : -velocity);
-  delay(20);
+  delay(10);
+  right_motor.write_rpm(robot_state == TURN ? -velocity : velocity);
+  delay(10);
 
   // READ MOTOR POSITION
-  int32_t rpm = left_motor.read_rpm();
-  int32_t error = velocity - rpm;
-  Serial.print("RPM: ");
-  Serial.print(rpm);
-  Serial.print(" Setpoint: ");
-  Serial.print(velocity);
-  Serial.print(" Err: ");
-  Serial.print(error);
+  int32_t rpm = right_motor.read_rpm();
+  int32_t pos = right_motor.read_angle();
+  int32_t error = setpoint.velocity * CMS_RPM - rpm;
+  Serial.print("time:");
+  Serial.print(time);
+  Serial.print(",dist goal:");
+  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(setpoint.velocity);
+  Serial.print(",Err:");
+  Serial.print(error / CMS_RPM);
   Serial.println("");
 
   // move on if at setpoint TODO: check that the error is low as well
@@ -87,6 +108,12 @@ void loop() {
       case FORWARD:
         robot_state = TURN;
         phase_start = (float)millis() / 1000.0;
+
+        // end (temp)
+        left_motor.write_rpm(0.0);
+        delay(10);
+        right_motor.write_rpm(0.0);
+        for (;;) {};
         break;
       case TURN:
         break;