From d3ed3f409353bfb4672d1262243f916c55fd80e1 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 25 Oct 2024 16:35:38 -0400
Subject: [PATCH] swerve termporarily on tracking loss

---
 src/robot.cpp | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/src/robot.cpp b/src/robot.cpp
index ca34c02..cf0a9c3 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -99,18 +99,29 @@ void Robot::EnterLineFollowing(float speed)
 
 int lineLostFrames = 24;
 
+float lastError = 0;
+
 void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
     {
-        float setpoint = 3.5;
+        float setpoint = rollingTurnRate > 0.1 ? 4 : 3;
 
         float lineError = setpoint - lineSensor.CalcError();
 
+
         float powErr = (lineError*lineError);
         powErr = (lineError < 0 ? -powErr : powErr);
 
-        float turnEffort = powErr * lining_kP;
+        float errDiff = lastError - lineError; 
+        if (lineSensor.LineDetected()) {
+            lastError = lineError;
+        } else {
+            errDiff = 0;
+            lineError = lastError > 0 ? 7:-7;
+        }
+
+        float turnEffort = powErr * lining_kP2 + lineError * lining_kP + errDiff * lining_kD;
 
         rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);