factored out trapezoidal calculations

robot_controller/robot_controller.ino

@@ -54,72 +54,6 @@ float CURRENT_POSITION = 0.0;
 float CURRENT_ROTATION = 0.0;
 float velocity = 40.0;
 
-struct Setpoint {
-  float velocity; // unitless
-  float position; // unitless
-  bool complete; // the setpoint will no longer change
-};
-
-// 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 setpoint = {0.0, 0.0};
-
-  float time_accelerating = max_acc / max_vel;
-  float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
-
-  if (2.0 * distance_while_accelerating > dist) {
-    // triangular
-    float peak_velocity_time = sqrt(dist/max_acc);
-    float peak_velocity = max_acc * peak_velocity_time;
-
-    if (time < peak_velocity_time) {
-      // accelerating
-      setpoint.velocity = max_acc * time;
-      setpoint.position = 0.5 * max_acc * time * time;
-    } else if (time < peak_velocity_time * 2.0) {
-      // slowing down
-      float time_decay = time - peak_velocity_time;
-      setpoint.velocity = peak_velocity - (time_decay * max_acc);
-      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 {
-      setpoint.velocity = 0;
-      setpoint.position = dist;
-      setpoint.complete = true;
-    }
-  } else {
-    // trapezoidal
-    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) {
-      // still accelerating
-      setpoint.velocity = time * max_acc;
-      setpoint.position = 0.5 * max_acc * time * time;
-    } else if (time < time_accelerating + cruise_time) {
-      // cruising
-      setpoint.velocity = max_vel;
-      setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
-    } else if (time < total_time) {
-      // slowing down
-      float time_decay = time - (time_accelerating + cruise_time);
-      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;
-    } else {
-      //done
-      setpoint.velocity = 0.0;
-      setpoint.position = dist;
-      setpoint.complete = true;
-    }
-  }
-
-  return setpoint;
-}
 
 void loop() {
   switch (robot_state) {

robot_controller/trapezoidal.ino

@@ -0,0 +1,68 @@
+// trapezoidal impl, not fuzzed
+
+struct Setpoint {
+  float velocity; // unitless
+  float position; // unitless
+  bool complete; // the setpoint will no longer change
+};
+
+// 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 setpoint = {0.0, 0.0};
+
+  float time_accelerating = max_acc / max_vel;
+  float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
+
+  if (2.0 * distance_while_accelerating > dist) {
+    // triangular
+    float peak_velocity_time = sqrt(dist/max_acc);
+    float peak_velocity = max_acc * peak_velocity_time;
+
+    if (time < peak_velocity_time) {
+      // accelerating
+      setpoint.velocity = max_acc * time;
+      setpoint.position = 0.5 * max_acc * time * time;
+    } else if (time < peak_velocity_time * 2.0) {
+      // slowing down
+      float time_decay = time - peak_velocity_time;
+      setpoint.velocity = peak_velocity - (time_decay * max_acc);
+      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 {
+      setpoint.velocity = 0;
+      setpoint.position = dist;
+      setpoint.complete = true;
+    }
+  } else {
+    // trapezoidal
+    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) {
+      // still accelerating
+      setpoint.velocity = time * max_acc;
+      setpoint.position = 0.5 * max_acc * time * time;
+    } else if (time < time_accelerating + cruise_time) {
+      // cruising
+      setpoint.velocity = max_vel;
+      setpoint.position = distance_while_accelerating + max_vel * (time - time_accelerating);
+    } else if (time < total_time) {
+      // slowing down
+      float time_decay = time - (time_accelerating + cruise_time);
+      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;
+    } else {
+      //done
+      setpoint.velocity = 0.0;
+      setpoint.position = dist;
+      setpoint.complete = true;
+    }
+  }
+
+  return setpoint;
+}