From bb330560710dcf37311341b49343c422c2a858c6 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 5 Feb 2026 09:46:03 -0500
Subject: [PATCH] follow sa path

---
 src/robot-nav.cpp | 14 +++++++-------
 src/robot.cpp     |  1 +
 src/robot.h       |  2 +-
 3 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index a9e8a86..3e1bc55 100644
--- a/src/robot-nav.cpp
+++ b/src/robot-nav.cpp
@@ -50,7 +50,7 @@ void Robot::SetDestination(const Pose& dest)
 
     Pose transformed;
 
-    // rotate clockwise
+    //// rotate clockwise
     transformed.x = dest.y;
     transformed.y = -dest.x;
     transformed.theta = dest.theta;
@@ -104,12 +104,12 @@ float boost(float input, float boost) {
 // y left
 //
 // goal y forward x right
-const int PATH_LEN=4;
+const int PATH_LEN=3;
 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),
+    Pose(0.0, 81.0, 0.0),
+    Pose(40.0, 40.0, M_PI/ 2.0),
+    Pose(0.0, 0.0, 0.0),
+
 };
 
 
@@ -136,7 +136,7 @@ void Robot::DriveToPoint(void)
             + sin(currPose.theta) * (destPose.y - currPose.y);
 
 
-        if (heading_err > 45.0) {
+        if (heading_err > 15.0) {
             forward_err = 0.0;
         }
 
diff --git a/src/robot.cpp b/src/robot.cpp
index c6f26fa..9991a11 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -80,6 +80,7 @@ void Robot::RobotLoop(void)
         if (chassis.buttonA.isPressed()) {
             //Pose dest = Pose(0.0,43.0, 0.0);
             //SetDestination(dest);
+            delay(1000);
             DrivePath();
         }
         if (chassis.buttonB.isPressed()) {
diff --git a/src/robot.h b/src/robot.h
index 01ff4da..36dfa53 100644
--- a/src/robot.h
+++ b/src/robot.h
@@ -6,7 +6,7 @@ const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (
 const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)
 
 const float LONGITUDINAL_kP = 4.0;
-const float LATERAL_kP = 1.5 * RAD_TO_DEG;
+const float LATERAL_kP = 1.7 * RAD_TO_DEG;
 
 const Pose offsetPose = Pose(100.0, 100.0, 0.0); // hack to avoid negatives