diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 9fcfd45..c2e6a10 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -8,5 +8,8 @@ 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);
+
 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 cf9f5fa..8fe78a2 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -53,7 +53,7 @@ void setup() {
 }
 
 void loop() {
-  float time = (float)millis() / 1000.0;
+  float time = (float)millis() / 1000.0 - phase_start;
   switch (robot_state) {
     case FORWARD:
       setpoint = trapezoidal_planner(&forward, time);
@@ -66,7 +66,7 @@ void loop() {
 
 
 
-  left_motor.write_rpm(velocity);
+  left_motor.write_rpm(setpoint.velocity * CMS_RPM);
   //right_motor.write_rpm(robot_state == TURN ? velocity : -velocity);
   delay(20);
 
diff --git a/robot_controller/trapezoidal.ino b/robot_controller/trapezoidal.ino
index 9da6772..847d608 100644
--- a/robot_controller/trapezoidal.ino
+++ b/robot_controller/trapezoidal.ino
@@ -8,7 +8,7 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
 
   struct Setpoint setpoint = {0.0, 0.0};
 
-  float time_accelerating = max_acc / max_vel;
+  float time_accelerating = max_vel / max_acc;
   float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
 
   if (2.0 * distance_while_accelerating > dist) {
@@ -48,7 +48,7 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
     } else if (time < total_time) {
       // slowing down
       float time_decay = time - (time_accelerating + cruise_time);
-      setpoint.velocity = max_vel - time_decay * max_acc;
+      setpoint.velocity = max_vel - (time_decay * max_acc);
       setpoint.position = distance_while_accelerating + cruise_distance
         + (max_vel * time_decay)
         - 0.5 * max_acc * time_decay * time_decay;