diff --git a/lib/LineSensor/src/LineSensor.cpp b/lib/LineSensor/src/LineSensor.cpp index 4c22271..eadaace 100644 --- a/lib/LineSensor/src/LineSensor.cpp +++ b/lib/LineSensor/src/LineSensor.cpp @@ -2,6 +2,9 @@ #define DARK_THRESHOLD 500; +#define LINE_THRESHOLD 200; +#define INTERSECTION_THRESHOLD 500; + void LineSensor::Initialize(void) { for (int i = 0; i LINE_THRESHOLD; +} + float LineSensor::CalcError(void) { float sum_pos = 0; diff --git a/lib/LineSensor/src/LineSensor.h b/lib/LineSensor/src/LineSensor.h index 8d6ea20..2b9d4e0 100644 --- a/lib/LineSensor/src/LineSensor.h +++ b/lib/LineSensor/src/LineSensor.h @@ -12,9 +12,12 @@ protected: bool prevOnIntersection = false; + float AverageReflectance(); + public: LineSensor(void) {} void Initialize(void); float CalcError(void); // varies between 1 and 6 bool CheckIntersection(void); + bool LineDetected(void); }; diff --git a/src/robot.cpp b/src/robot.cpp index 8c333de..9dd0ae1 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -97,6 +97,9 @@ void Robot::EnterLineFollowing(float speed) robotState = ROBOT_LINING; } +int lineLostFrames = 7; +float lastError = 0; + void Robot::LineFollowingUpdate(void) { if(robotState == ROBOT_LINING) @@ -107,6 +110,21 @@ void Robot::LineFollowingUpdate(void) float turnEffort = lineError * lining_kP; chassis.SetTwist(baseSpeed, turnEffort); + float speed = baseSpeed; + + if (!lineSensor.LineDetected()) { + lineLostFrames -= 1; + if (lineLostFrames < 0) { + rollingTurnRate = 0; + turnEffort = 0; + speed = 0; + } + } + else { + lineLostFrames = 7; + } + + chassis.SetTwist(speed, turnEffort); } }