From 90acef4110e611bf845ea1b9a8a52d68a50f3bf0 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 5 Feb 2026 08:31:00 -0500
Subject: [PATCH] impl point tracking

switched longitudinal error equation to not care about lateral offsets
---
 lib/Chassis/src/chassis.h |  1 +
 lib/Chassis/src/utils.cpp |  2 +-
 src/robot-nav.cpp         | 20 ++++++++++++++++----
 src/robot.cpp             |  5 +++++
 src/robot.h               |  2 +-
 5 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h
index 8b58196..7487ce5 100644
--- a/lib/Chassis/src/chassis.h
+++ b/lib/Chassis/src/chassis.h
@@ -37,6 +37,7 @@ public:
     static void Timer4OverflowISRHandler(void);
 
 public:
+    Romi32U4ButtonA buttonA;
     Romi32U4ButtonB buttonB;
     Romi32U4ButtonC buttonC;
 
diff --git a/lib/Chassis/src/utils.cpp b/lib/Chassis/src/utils.cpp
index 58efd54..5c1b161 100644
--- a/lib/Chassis/src/utils.cpp
+++ b/lib/Chassis/src/utils.cpp
@@ -2,7 +2,7 @@
 
 void TeleplotPrint(const char* var, float value)
 {
-    Serial.print('>');
+    //Serial.print('>');
     Serial.print(var);
     Serial.print(':');
     Serial.print(value);
diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index bb8e158..63860fb 100644
--- a/src/robot-nav.cpp
+++ b/src/robot-nav.cpp
@@ -18,8 +18,15 @@ void Robot::UpdatePose(const Pose& delta)
     currPose.theta += delta.theta;
 
 #ifdef __NAV_DEBUG__
-    TeleplotPrint("x", currPose.x);
-    TeleplotPrint("y", currPose.y);
+    Serial.print("pos");
+    Serial.print(':');
+    Serial.print(currPose.x);
+    Serial.print(':');
+    Serial.print(currPose.y);
+    Serial.print('|xy\n');
+
+    //TeleplotPrint("x", currPose.x);
+    //TeleplotPrint("y", currPose.y);
     TeleplotPrint("theta", currPose.theta * 180.0 / PI);
 #endif
 }
@@ -75,17 +82,22 @@ void Robot::DriveToPoint(void)
         if (SquareDistance() < TARGET_SQ_DIST) {
             target_heading = destPose.theta;
         } else {
-            target_heading = atan2(destPose.x - currPose.x, destPose.y - currPose.y);
+            target_heading = atan2(destPose.y - currPose.y, destPose.x - currPose.x);
         }
 
         float heading_err = RadError(currPose.theta, target_heading);
-        float forward_err = cos(heading_err) * sqrt(SquareDistance());
+        float forward_err = cos(currPose.theta) * (destPose.x - currPose.x)
+            + sin(currPose.theta) * (destPose.y - currPose.y);
 
         float turn_effort = heading_err * LATERAL_kP;
         float forward_effort = forward_err * LONGITUDINAL_kP;
 
 #ifdef __NAV_DEBUG__
         // Print useful stuff here.
+
+        TeleplotPrint("heading_err", heading_err * RAD_TO_DEG);
+        TeleplotPrint("linear_err", forward_err);
+        TeleplotPrint("linear_dist", SquareDistance());
 #endif
 
         chassis.SetMotorEfforts(forward_effort - turn_effort, forward_effort + turn_effort);
diff --git a/src/robot.cpp b/src/robot.cpp
index 27f8895..6c13b1a 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -1,4 +1,5 @@
 #include "robot.h"
+#include "utils.h"
 
 void Robot::InitializeRobot(void)
 {
@@ -76,6 +77,10 @@ void Robot::RobotLoop(void)
         UpdatePose(delta);
         //chassis.SetMotorEfforts(220,-220);
 
+        if (chassis.buttonA.isPressed()) {
+            Pose dest(0.0, 10.0, 0.0);
+            SetDestination(dest);
+        }
         if (chassis.buttonB.isPressed()) {
             EnterCalibTurn();
         }
diff --git a/src/robot.h b/src/robot.h
index 4f8d975..df61f58 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 = 3.0;
-const float LATERAL_kP = 3.0 * DEG_TO_RAD;
+const float LATERAL_kP = 3.0 * RAD_TO_DEG;
 
 class Robot
 {