track white lines better

lib/LineSensor/src/LineSensor.cpp

@@ -23,7 +23,12 @@ float LineSensor::AverageReflectance(void)
 }
 
 bool LineSensor::LineDetected() {
+#ifdef DARK
     return AverageReflectance() > LINE_THRESHOLD;
+#endif
+#ifndef DARK
+    return AverageReflectance() < 200;
+#endif
 }
 
 float LineSensor::CalcError(void) 
@@ -32,6 +37,9 @@ float LineSensor::CalcError(void)
     float sum = 0;
     for (int i = 0; i<sensorCount; i++) {
         int ret = analogRead(sensors[i]);
+#ifndef DARK
+        ret = 1000 - ret;
+#endif
         sum_pos += ret * (i+1);
         sum += ret;
 #ifdef __TRACK_DEBUG__
@@ -41,6 +49,8 @@ float LineSensor::CalcError(void)
     }
     float pos = sum_pos / sum;
 #ifdef __TRACK_DEBUG__
+    Serial.print(AverageReflectance());
+    Serial.print(" ");
     Serial.println(pos);
 #endif
     return pos;
@@ -49,5 +59,10 @@ float LineSensor::CalcError(void)
 
 bool LineSensor::CheckIntersection(void)
 {
+#ifdef DARK
     return AverageReflectance() > INTERSECTION_THRESHOLD;
+#endif
+#ifndef DARK
+    return AverageReflectance() < 100;
+#endif
 }

lib/LineSensor/src/LineSensor.h

@@ -2,19 +2,17 @@
 
 #include <Arduino.h>
 
-#define FIRST_LINE_SENSOR A0
-
 class LineSensor
 {
 protected:
     const static uint8_t sensorCount = 6;
-    const byte sensors[sensorCount] = {A0,A7,A2,A3,A4,A6};
+    const byte sensors[sensorCount] = {A0,A11,A2,A3,A4,A6};
 
     bool prevOnIntersection = false;
 
+public:
     float AverageReflectance();
 
-public:
     LineSensor(void) {}
     void Initialize(void);
     float CalcError(void); // varies between 1 and 6

src/robot.cpp

@@ -135,7 +135,12 @@ void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
     {
+#ifdef DARK
         float setpoint = rollingTurnRate > 0.1 ? 4 : 3;
+#endif
+#ifndef DARK
+        float setpoint = 3.5;
+#endif
 
         float lineError = setpoint - lineSensor.CalcError();