From ac0b2050e24091248dae50bcb3c4a10783830ef6 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Sat, 6 Dec 2025 14:00:22 -0500
Subject: [PATCH] added constants

---
 robot_controller/robot_controller.ino | 87 ++++++++++++++++++++-------
 1 file changed, 66 insertions(+), 21 deletions(-)

diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index d9bf538..e5f07e6 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -3,6 +3,17 @@
 #include <smartmotor.h>
 #include <SMC_gains.h>
 
+const float ACCEL_LIMIT = 4.0; // cm/s/s
+const float VEL_LIMIT = 16.0; // cm/s
+const float BACKLASH_RIGHT = 0.0; // degrees
+const float BACKLASH_LEFT = 0.0; // degrees
+const float WHEEL_DIAMETER = 6.37; // cm
+const float WHEEL_TO_WHEEL = 0.0; // cm
+
+const float FORWARD_DISTANCE = 100.0; // cm
+const float TURN_AMOUNT = 180.0; // degrees
+const float RETURN_DISTANCE = 100.0; // cm
+
 SmartMotor left_motor(0x0A); 
 SmartMotor right_motor(0x0B); 
 
@@ -14,11 +25,10 @@ void setup() {
     Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
     
     // TUNE POSITION PID
-    left_motor.tune_pos_pid(0.65,0.060,0.065); 
-    right_motor.tune_pos_pid(0.65,0.060,0.065); 
+    left_motor.tune_vel_pid(9.0, 0.0,0.0,0.0); 
+    right_motor.tune_vel_pid(1.0, 0.65,0.060,0.065); 
     delay(1000);
 
-
     // SET MOTOR POSITION
     //int32_t angle=360;
     //uint8_t status= left_motor.write_angle(angle);
@@ -31,30 +41,65 @@ void setup() {
     Serial.println(" deg");
 }
 
-int32_t angle = 0;
-
 enum ROBOT_STATE {
   FORWARD,
   TURN,
-  REVERSE
+  RETURN,
+  COMPLETE,
 };
 
 enum ROBOT_STATE robot_state = FORWARD;
 
-void loop() {
-    angle = (millis() / 2000) * 360;
-    left_motor.write_angle(angle);
-    right_motor.write_angle(angle);
-    delay(20);
+float CURRENT_POSITION = 0.0;
+float CURRENT_ROTATION = 0.0;
+float velocity = 40.0;
 
-    // READ MOTOR POSITION
-    int32_t pos = left_motor.read_angle();
-    int32_t error = angle - pos;
-    Serial.print("Pos: ");
-    Serial.print(pos);
-    Serial.print(" Setpoint: ");
-    Serial.print(angle);
-    Serial.print(" Err: ");
-    Serial.print(error);
-    Serial.println("");
+struct Setpoint {
+  float velocity;
+  float position;
+};
+
+// 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) {
+  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
+  } else {
+    // trapezoidal
+  }
+
+
+
+  struct Setpoint setpoint = {0.0, 0.0};
+  return setpoint;
+}
+
+void loop() {
+  switch (robot_state) {
+    case FORWARD:
+      break;
+    case TURN:
+      break;
+    case RETURN:
+      break;
+  }
+
+
+
+  left_motor.write_rpm(velocity);
+  //right_motor.write_rpm(robot_state == TURN ? velocity : -velocity);
+  delay(20);
+
+  // READ MOTOR POSITION
+  int32_t rpm = left_motor.read_rpm();
+  int32_t error = velocity - rpm;
+  Serial.print("RPM: ");
+  Serial.print(rpm);
+  Serial.print(" Setpoint: ");
+  Serial.print(velocity);
+  Serial.print(" Err: ");
+  Serial.print(error);
+  Serial.println("");
 }