From ab507bb68a3d91003421f2eeb81db7c865bc0764 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 5 Feb 2026 08:35:43 -0500
Subject: [PATCH] add static friction feedforward

---
 src/robot-nav.cpp | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp
index 63860fb..35857fa 100644
--- a/src/robot-nav.cpp
+++ b/src/robot-nav.cpp
@@ -73,6 +73,18 @@ bool Robot::CheckReachedDestination(void)
         && RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
 }
 
+float boost(float input, float boost) {
+    if (input == 0.0) {
+        return input;
+    }
+    if (input > 0.0) {
+        input += boost;
+    } else {
+        input -= boost;
+    }
+    return input;
+}
+
 void Robot::DriveToPoint(void)
 {
     if(robotState == ROBOT_DRIVE_TO_POINT)
@@ -100,7 +112,9 @@ void Robot::DriveToPoint(void)
         TeleplotPrint("linear_dist", SquareDistance());
 #endif
 
-        chassis.SetMotorEfforts(forward_effort - turn_effort, forward_effort + turn_effort);
+        //chassis.SetMotorEfforts(20, 20);
+        float boost_f = 15.0;
+        chassis.SetMotorEfforts(boost(forward_effort - turn_effort,boost_f), boost(forward_effort + turn_effort, boost_f));
     }
 }