From 17707897d6fd5cce448113396a2f76ac0e114940 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Sat, 6 Dec 2025 14:19:44 -0500
Subject: [PATCH] banged out trapezoidal impl

not fuzzed, likely bugged
---
 robot_controller/robot_controller.ino | 57 ++++++++++++++++++++++++---
 1 file changed, 51 insertions(+), 6 deletions(-)

diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index e5f07e6..c1bb993 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -55,24 +55,69 @@ float CURRENT_ROTATION = 0.0;
 float velocity = 40.0;
 
 struct Setpoint {
-  float velocity;
-  float position;
+  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
-struct Setpoint trapezoidal_velocity(float max_vel, float, max_acc, float dist, float time) {
+// 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(distance/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 * A * 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 = distance - 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 < time_total) {
+      // 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 = dest;
+      setpoint.complete = true;
+    }
   }
 
-
-
-  struct Setpoint setpoint = {0.0, 0.0};
   return setpoint;
 }