From fb1667b1e20256c0ef18ce2fc66763b90c396961 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 10 Dec 2025 14:43:01 -0500
Subject: [PATCH] goes and turns around

---
 robot_controller/constants.h          | 13 ++++----
 robot_controller/robot_controller.ino | 47 ++++++++++++++++-----------
 2 files changed, 35 insertions(+), 25 deletions(-)

diff --git a/robot_controller/constants.h b/robot_controller/constants.h
index 4498c1c..d708666 100644
--- a/robot_controller/constants.h
+++ b/robot_controller/constants.h
@@ -13,16 +13,17 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
 // degrees to centimeters
 const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
 
-const float FORWARD_DISTANCE = 10.0; // cm
-const float TURN_AMOUNT = 175.0; // degrees
+const float FORWARD_DISTANCE = 73.0; // cm
+//const float FORWARD_DISTANCE = 10.0; // cm
+const float TURN_AMOUNT = 146.0; // degrees
 const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
 const float RETURN_DISTANCE = 100.0; // cm
 
-const float FF_ACCEL = 2.3; // motor acceleration feedforward
-const float FF_VEL = 0.7; // motor velocity feedforward
+const float FF_ACCEL = 2.5; // motor acceleration feedforward
+const float FF_VEL = 0.9; // motor velocity feedforward
 const float FF_STAT = 16.4; // motor static friction
-const float KP = 2.0; // proportional
+const float KP = 2.7; // proportional
 const float KI = 0.5; // integral
 const float KD = 0.05; // derivative
 const float KPP = 0.25; // position proportional
-const float KPI = 0.05; // position integral
+const float KPI = 0.07; // position integral
diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
index 1f99668..de98ee2 100644
--- a/robot_controller/robot_controller.ino
+++ b/robot_controller/robot_controller.ino
@@ -106,35 +106,44 @@ void loop() {
     ((left_velocity > 0.0) ? FF_STAT : -FF_STAT);
 
   // arbitrary feedforward with on-board velocity PID
-  write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
-  write_rpm_ff(&left_motor, left_velocity * CMS_RPM  , feedforward_left);
+	if (setpoint.complete) {
+		right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
+		left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
+	} else {
+		write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right);
+		write_rpm_ff(&left_motor, left_velocity * CMS_RPM  , feedforward_left);
+	}
 
   // send telemetry
   int32_t rpm = left_motor.read_rpm();
   int32_t pos = left_motor.read_angle();
   int32_t error = setpoint.velocity * CMS_RPM - rpm;
-  Serial.print("ff:");
-  Serial.print(left_effort);
-  Serial.print(",time:");
-  Serial.print(time);
-  Serial.print(",distgoal:");
-  Serial.print(setpoint.position);
-  Serial.print(",dist:");
-  Serial.print(pos * DEG_CM);
-  Serial.print(",cms/s:");
-  Serial.print(rpm / CMS_RPM);
-  Serial.print(",setvel:");
-  Serial.print(setpoint.velocity);
-  Serial.print(",setacc:");
-  Serial.print(setpoint.acceleration);
-  Serial.print(",Err:");
-  Serial.print(error / CMS_RPM);
+  Serial.print("el:");
+  Serial.print(pos_err_left);
+  Serial.print(",er:");
+  Serial.print(pos_err_right);
+  //Serial.print("ff:");
+  //Serial.print(left_effort);
+  //Serial.print(",time:");
+  //Serial.print(time);
+  //Serial.print(",distgoal:");
+  //Serial.print(setpoint.position);
+  //Serial.print(",dist:");
+  //Serial.print(pos * DEG_CM);
+  //Serial.print(",cms/s:");
+  //Serial.print(rpm / CMS_RPM);
+  //Serial.print(",setvel:");
+  //Serial.print(setpoint.velocity);
+  //Serial.print(",setacc:");
+  //Serial.print(setpoint.acceleration);
+  //Serial.print(",Err:");
+  //Serial.print(error / CMS_RPM);
   Serial.println("");
 
   // total wheel offness in degrees
   float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
   // move on if at setpoint TODO: give up after timeout
-  if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 3.0)) {
+  if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 7.0)) {
     // rehome
     right_home += (setpoint.position / DEG_CM);
     left_home += (turnsig * setpoint.position / DEG_CM);