From e38cb63c7f40b541d11212cf9b93510721a09ca2 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 24 Oct 2024 15:31:29 -0400
Subject: [PATCH] implemented lining mode

---
 src/robot.cpp | 5 +++--
 src/robot.h   | 2 ++
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/robot.cpp b/src/robot.cpp
index 7b57679..8c333de 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -102,8 +102,9 @@ void Robot::LineFollowingUpdate(void)
     if(robotState == ROBOT_LINING) 
     {
         // TODO: calculate the error in CalcError(), calc the effort, and update the motion
-        int16_t lineError = lineSensor.CalcError();
-        float turnEffort = 0;
+        float lineError = 3.5 - lineSensor.CalcError();
+        Serial.println(lineError);
+        float turnEffort = lineError * lining_kP;
 
         chassis.SetTwist(baseSpeed, turnEffort);
     }
diff --git a/src/robot.h b/src/robot.h
index d7f6f33..3d7697c 100644
--- a/src/robot.h
+++ b/src/robot.h
@@ -3,6 +3,8 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 
+static float lining_kP = 1.0;
+
 class Robot
 {
 protected: