switched to using six line sensors

lib/LineSensor/src/LineSensor.cpp

@@ -4,13 +4,29 @@
 
 void LineSensor::Initialize(void)
 {
-    pinMode(leftSensorPin, INPUT);
+    for (int i = 0; i<sensorCount; i++) {
-    pinMode(rightSensorPin, INPUT);
+        pinMode(sensors[i], INPUT);
+    }
 }
 
-int16_t LineSensor::CalcError(void) 
+float LineSensor::CalcError(void) 
 { 
-    return analogRead(rightSensorPin) - analogRead(leftSensorPin); 
+    float sum_pos = 0;
+    float sum = 0;
+    for (int i = 0; i<sensorCount; i++) {
+        int ret = analogRead(sensors[i]);
+        sum_pos += ret * (i+1);
+        sum += ret;
+#ifdef __TRACK_DEBUG__
+        Serial.print(ret);
+        Serial.print(" ");
+#endif
+    }
+    float pos = sum_pos / sum;
+#ifdef __TRACK_DEBUG__
+    Serial.println(pos);
+#endif
+    return pos;
 }
     
 

lib/LineSensor/src/LineSensor.h

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