diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index c1bb993..5987506 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -63,7 +63,7 @@ struct Setpoint {
 // returns the position and velocity at the given time on a trapezoidal motion plan
 // this could be baked if too computationally expensive
 // not fully fuzzed
-struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, float time) {
+struct Setpoint trapezoidal_planner(float max_vel, float max_acc, float dist, float time) {
   struct Setpoint setpoint = {0.0, 0.0};
 
   float time_accelerating = max_acc / max_vel;
@@ -71,7 +71,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
 
   if (2.0 * distance_while_accelerating > dist) {
     // triangular
-    float peak_velocity_time = sqrt(distance/max_acc);
+    float peak_velocity_time = sqrt(dist/max_acc);
     float peak_velocity = max_acc * peak_velocity_time;
 
     if (time < peak_velocity_time) {
@@ -82,7 +82,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
       // slowing down
       float time_decay = time - peak_velocity_time;
       setpoint.velocity = peak_velocity - (time_decay * max_acc);
-      setpoint.position = (0.5 * A * peak_velocity_time * peak_velocity_time) // acceleration phase
+      setpoint.position = (0.5 * max_acc * peak_velocity_time * peak_velocity_time) // acceleration phase
         + peak_velocity * time_decay 
         - 0.5 * max_acc * time_decay * time_decay;
     } else {
@@ -92,7 +92,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
     }
   } else {
     // trapezoidal
-    float cruise_distance = distance - 2.0 * distance_while_accelerating;
+    float cruise_distance = dist - 2.0 * distance_while_accelerating;
     float cruise_time = cruise_distance / max_vel;
     float total_time = 2 * time_accelerating + cruise_time;
     if (time < time_accelerating) {
@@ -103,7 +103,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
       // cruising
       setpoint.velocity = max_vel;
       setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
-    } else if (time < time_total) {
+    } else if (time < total_time) {
       // slowing down
       float time_decay = time - (time_accelerating + cruise_time);
       setpoint.velocity = max_vel - time_decay * max_acc;
@@ -113,7 +113,7 @@ struct Setpoint trapezoidal_planner(float max_vel, float, max_acc, float dist, f
     } else {
       //done
       setpoint.velocity = 0.0;
-      setpoint.position = dest;
+      setpoint.position = dist;
       setpoint.complete = true;
     }
   }