stop on line loss

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<sensorCount; i++) {
@@ -9,6 +12,20 @@ void LineSensor::Initialize(void)
     }
 }
 
+float LineSensor::AverageReflectance(void) 
+{ 
+    float sum = 0;
+    for (int i = 0; i<sensorCount; i++) {
+        int ret = analogRead(sensors[i]);
+        sum += ret;
+    }
+    return sum/sensorCount;
+}
+
+bool LineSensor::LineDetected() {
+    return AverageReflectance() > LINE_THRESHOLD;
+}
+
 float LineSensor::CalcError(void) 
 { 
     float sum_pos = 0;

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);
 };

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);
     }
 }