From 81bec195f01d712c7ce04fd95d9a9df18db65305 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Sat, 6 Dec 2025 14:37:10 -0500
Subject: [PATCH] factored out trapezoidal config to struct

---
 robot_controller/robot_controller.ino | 50 +++++++++++++++++++--------
 robot_controller/trapezoidal.ino      | 34 +++++++++++++++++-
 2 files changed, 69 insertions(+), 15 deletions(-)

diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index 18e6148..82f0ad1 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -17,6 +17,23 @@ const float RETURN_DISTANCE = 100.0; // cm
 SmartMotor left_motor(0x0A); 
 SmartMotor right_motor(0x0B); 
 
+enum ROBOT_STATE {
+  FORWARD,
+  TURN,
+  RETURN,
+  COMPLETE,
+};
+
+float CURRENT_POSITION = 0.0;
+float CURRENT_ROTATION = 0.0;
+struct Setpoint setpoint; 
+float velocity = 40.0;
+// start of current state
+float phase_start;
+enum ROBOT_STATE robot_state;
+
+struct Trapezoidal forward = {VEL_LIMIT, ACCEL_LIMIT, FORWARD_DISTANCE};
+
 void setup() {
     // INIT SERIAL
     Serial.begin(115200);
@@ -39,25 +56,16 @@ void setup() {
     Serial.print("Pos: ");
     Serial.print(pos);
     Serial.println(" deg");
+
+    phase_start = (float)millis() / 1000.0;
+    robot_state = FORWARD;
 }
 
-enum ROBOT_STATE {
-  FORWARD,
-  TURN,
-  RETURN,
-  COMPLETE,
-};
-
-enum ROBOT_STATE robot_state = FORWARD;
-
-float CURRENT_POSITION = 0.0;
-float CURRENT_ROTATION = 0.0;
-float velocity = 40.0;
-
-
 void loop() {
+  float time = (float)millis() / 1000.0;
   switch (robot_state) {
     case FORWARD:
+      setpoint = trapezoidal_planner(&forward, time);
       break;
     case TURN:
       break;
@@ -81,4 +89,18 @@ void loop() {
   Serial.print(" Err: ");
   Serial.print(error);
   Serial.println("");
+
+  // move on if at setpoint TODO: check that the error is low as well
+  if (setpoint.completed) {
+    switch (robot_state) {
+      case FORWARD:
+        robot_state = TURN;
+        phase_start = (float)millis() / 1000.0;
+        break;
+      case TURN:
+        break;
+      case RETURN:
+        break;
+    }
+  }
 } 
diff --git a/robot_controller/trapezoidal.ino b/robot_controller/trapezoidal.ino
index 3a4d5be..87799bc 100644
--- a/robot_controller/trapezoidal.ino
+++ b/robot_controller/trapezoidal.ino
@@ -1,5 +1,12 @@
 // trapezoidal impl, not fuzzed
 
+// unitless trapezoidal
+struct Trapezoidal {
+  float max_vel;
+  float max_acc;
+  float dist;
+};
+
 struct Setpoint {
   float velocity; // unitless
   float position; // unitless
@@ -9,7 +16,11 @@ 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(struct Trapezoidal* trapezoidal, float time) {
+  float max_vel = trapezoidal->max_vel;
+  float max_acc = trapezoidal->max_acc;
+  float dist = trapezoidal->dist;
+
   struct Setpoint setpoint = {0.0, 0.0};
 
   float time_accelerating = max_acc / max_vel;
@@ -66,3 +77,24 @@ struct Setpoint trapezoidal_planner(float max_vel, float max_acc, float dist, fl
 
   return setpoint;
 }
+
+float trapezoidal_time(struct Trapezoidal* trapezoidal) {
+  float max_vel = trapezoidal->max_vel;
+  float max_acc = trapezoidal->max_acc;
+  float dist = trapezoidal->dist;
+
+  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);
+    return peak_velocity_time * 2.0;
+  } 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;
+    return total_time;
+  }
+}