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) {