From cbf38e4fd68d3595c9a80fcaaa2bb961139e71f6 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 25 Oct 2024 13:29:23 -0400
Subject: [PATCH] slow down on turns

---
 src/robot.cpp | 18 ++++++++++++------
 src/robot.h   |  6 +++++-
 2 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/src/robot.cpp b/src/robot.cpp
index 9dd0ae1..7ee5060 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -98,20 +98,26 @@ void Robot::EnterLineFollowing(float speed)
 }
 
 int lineLostFrames = 7;
-float lastError = 0;
 
 void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
     {
-        // TODO: calculate the error in CalcError(), calc the effort, and update the motion
-        float lineError = 3.5 - lineSensor.CalcError();
-        Serial.println(lineError);
-        float turnEffort = lineError * lining_kP;
+        float setpoint = 3.5;
+
+        float lineError = setpoint - lineSensor.CalcError();
+
+        float powErr = (lineError*lineError);
+        powErr = (lineError < 0 ? -powErr : powErr);
+
+        float turnEffort = powErr * lining_kP;
+
+        rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);
 
-        chassis.SetTwist(baseSpeed, turnEffort);
         float speed = baseSpeed;
 
+        speed *= 1 - (abs(rollingTurnRate) * KTurnRate);
+
         if (!lineSensor.LineDetected()) { 
             lineLostFrames -= 1;
             if (lineLostFrames < 0) {
diff --git a/src/robot.h b/src/robot.h
index 3d7697c..1823c2a 100644
--- a/src/robot.h
+++ b/src/robot.h
@@ -3,7 +3,7 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 
-static float lining_kP = 1.0;
+static float lining_kP = 1.24;
 
 class Robot
 {
@@ -57,6 +57,10 @@ protected:
     /* baseSpeed is used to drive at a given speed while, say, line following.*/
     float baseSpeed = 0;
 
+    float rollingTurnRate = 0;
+    float turnRateDamp = 0.96; // the proportion of the turn rate retained each loop
+    float KTurnRate = 0.25; // slowdown coefficient
+
     /**
      * For tracking the motion of the Romi. We keep track of the intersection we came
      * from and the one we're headed to. You'll program in the map in handleIntersection()