From b2d0b01232f59a1030b6f563e5fa93e59e56c2a4 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 5 Feb 2026 09:26:22 -0500
Subject: [PATCH] tuned path sequencing

PATH is in lab coordinates, then transformed
---
 src/robot-nav.cpp | 40 +++++++++++++++++++++++++++++++++++-----
 src/robot.cpp     |  6 +++---
 src/robot.h       |  6 ++++--
 3 files changed, 42 insertions(+), 10 deletions(-)

diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index c78f811..a9e8a86 100644
--- a/src/robot-nav.cpp
+++ b/src/robot-nav.cpp
@@ -46,7 +46,21 @@ void Robot::SetDestination(const Pose& dest)
     Serial.print(dest.y);
     Serial.print('\n');
 
-    destPose = dest;
+    // transform to board space
+
+    Pose transformed;
+
+    // rotate clockwise
+    transformed.x = dest.y;
+    transformed.y = -dest.x;
+    transformed.theta = dest.theta;
+
+    // add offset
+    transformed.x += offsetPose.x;
+    transformed.y += offsetPose.y;
+    transformed.theta += offsetPose.theta;
+
+    destPose = transformed;
     robotState = ROBOT_DRIVE_TO_POINT;
 }
 
@@ -86,13 +100,21 @@ float boost(float input, float boost) {
     return input;
 }
 
-Pose PATH[2] = {
-    Pose(43.0,0,0),
-    Pose(0,0,0),
+// x forward
+// y left
+//
+// goal y forward x right
+const int PATH_LEN=4;
+Pose PATH[PATH_LEN] = {
+    Pose(0.0, 43.0, 0.0),
+    Pose(43.0, 0.0, 0.0),
+    Pose(43.0, 43.0, 0.0),
+    Pose(0.0,0.0,0.0),
 };
 
+
 void Robot::DrivePath(void) {
-    currPose = Pose(); // clear pose
+    currPose = offsetPose; // clear pose
     index = 0;
     SetDestination(PATH[index]);
 }
@@ -113,9 +135,16 @@ void Robot::DriveToPoint(void)
         float forward_err = cos(currPose.theta) * (destPose.x - currPose.x)
             + sin(currPose.theta) * (destPose.y - currPose.y);
 
+
+        if (heading_err > 45.0) {
+            forward_err = 0.0;
+        }
+
         float turn_effort = heading_err * LATERAL_kP;
         float forward_effort = forward_err * LONGITUDINAL_kP;
 
+        turn_effort = max(min(turn_effort, 100.0), -100.0); // constrain turn_effort
+
 #ifdef __NAV_DEBUG__
         // Print useful stuff here.
 
@@ -138,6 +167,7 @@ void Robot::HandleDestination(void)
     index += 1;
     if (index == len(PATH)) { 
         EnterIdleState();
+        return;
     }
 
     TeleplotPrint("path_index", index);
diff --git a/src/robot.cpp b/src/robot.cpp
index 82c48d7..c6f26fa 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -78,9 +78,9 @@ void Robot::RobotLoop(void)
         //chassis.SetMotorEfforts(220,-220);
 
         if (chassis.buttonA.isPressed()) {
-            Pose dest(0.0, 10.0, 0.0);
-            index = 0;
-            SetDestination(dest);
+            //Pose dest = Pose(0.0,43.0, 0.0);
+            //SetDestination(dest);
+            DrivePath();
         }
         if (chassis.buttonB.isPressed()) {
             EnterCalibTurn();
diff --git a/src/robot.h b/src/robot.h
index 4f79bdf..01ff4da 100644
--- a/src/robot.h
+++ b/src/robot.h
@@ -5,8 +5,10 @@
 const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (cm)
 const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)
 
-const float LONGITUDINAL_kP = 3.0;
-const float LATERAL_kP = 3.0 * RAD_TO_DEG;
+const float LONGITUDINAL_kP = 4.0;
+const float LATERAL_kP = 1.5 * RAD_TO_DEG;
+
+const Pose offsetPose = Pose(100.0, 100.0, 0.0); // hack to avoid negatives
 
 class Robot
 {