diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index d638944..45c63fe 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -36,25 +36,12 @@ void setup() {
     // 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); 
+    right_motor.tune_vel_pid(0.9, 3.7,0.3,0.0); 
     delay(10);
-    right_motor.set_direction(PIDDirection::DIRECT);
+    //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
-    //int32_t angle=360;
-    //uint8_t status= left_motor.write_angle(angle);
-    //delay(1000);
-
     // READ MOTOR POSITION
     int32_t pos = left_motor.read_angle();
     Serial.print("Pos: ");
@@ -81,7 +68,7 @@ void loop() {
 
   left_motor.write_rpm(setpoint.velocity * CMS_RPM);
   delay(10);
-  right_motor.write_rpm(robot_state == TURN ? -velocity : velocity);
+  right_motor.write_rpm((robot_state == TURN ? velocity : -velocity) * CMS_RPM);
   delay(10);
 
   // READ MOTOR POSITION