From d3b9bd3ae50e543d6ac4e3d078e988fe8e1fffbb Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 25 Oct 2024 16:31:08 -0400
Subject: [PATCH] go straight on intersections

---
 lib/LineSensor/src/LineSensor.cpp | 14 ++------------
 src/robot.cpp                     |  5 +++++
 2 files changed, 7 insertions(+), 12 deletions(-)

diff --git a/lib/LineSensor/src/LineSensor.cpp b/lib/LineSensor/src/LineSensor.cpp
index eadaace..2a800e5 100644
--- a/lib/LineSensor/src/LineSensor.cpp
+++ b/lib/LineSensor/src/LineSensor.cpp
@@ -49,15 +49,5 @@ float LineSensor::CalcError(void)
 
 bool LineSensor::CheckIntersection(void)
 {
-    bool retVal = false;
-
-    bool isLeftDark = analogRead(leftSensorPin) > DARK_THRESHOLD;
-    bool isRightDark = analogRead(rightSensorPin) > DARK_THRESHOLD;
-
-    bool onIntersection = isLeftDark && isRightDark;
-    if(onIntersection && !prevOnIntersection) retVal = true;
-
-    prevOnIntersection = onIntersection;
-
-    return retVal;
-}
\ No newline at end of file
+    return AverageReflectance() > INTERSECTION_THRESHOLD;
+}
diff --git a/src/robot.cpp b/src/robot.cpp
index 7ee5060..61d712e 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -118,6 +118,11 @@ void Robot::LineFollowingUpdate(void)
 
         speed *= 1 - (abs(rollingTurnRate) * KTurnRate);
 
+        if (lineSensor.CheckIntersection()) {
+            turnEffort = 0;
+            speed *= 0.8;
+        }
+
         if (!lineSensor.LineDetected()) { 
             lineLostFrames -= 1;
             if (lineLostFrames < 0) {