implemented lining mode

lib/LineSensor/src/LineSensor.cpp

@@ -4,13 +4,29 @@
 
 void LineSensor::Initialize(void)
 {
-    pinMode(leftSensorPin, INPUT);
-    pinMode(rightSensorPin, INPUT);
+    for (int i = 0; i<sensorCount; i++) {
+        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 RIGHT_LINE_SENSOR A4
+#define FIRST_LINE_SENSOR A0
 
 class LineSensor
 {
 protected:
-    uint8_t leftSensorPin = LEFT_LINE_SENSOR;
-    uint8_t rightSensorPin = RIGHT_LINE_SENSOR;
+    const static uint8_t sensorCount = 6;
+    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);
-};
+};

lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h

@@ -24,15 +24,18 @@ protected:
     enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
     volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
 
-    // TODO: After you tune your motors, set the gains here.
-    float Kp = 1;
-    float Ki = 0;
+    float Kp = 2.50; // proportional to error
+    float Ki = 0.03;
     float Kd = 0;
-    float Kf = 0;
+    float Kf = 3.00; // proportional to setpoint
+    int16_t C = 22; // constant added to effort
 
     // Used to keep track of the target speed, in counts / interval.
     float targetSpeed = 0;
 
+    // maximum accelration in counts/interval
+    float TRAPEZOIDAL_RAMP_RATE = 0;
+
     /**
      * This is the speed of the motor, in "encoder counts / encoder interval".
      * The encoder interval is set in Robot::InitializeMotorControlTimer.
@@ -125,6 +128,11 @@ protected:
 
             sumError += error; 
 
+            float maxSumError = maxEffort/Ki;
+
+            sumError = max(sumError, -maxSumError);
+            sumError = min(sumError, maxSumError);
+
             float errorDiff = error - prevError;
 
             prevError = error;
@@ -132,6 +140,18 @@ protected:
             // Calculate the effort from the PID gains
             int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff + Kf * targetSpeed;
 
+            // increase effort to match static friction
+            if (effort > 0) {
+                effort += C;
+            } else if (effort < 0) {
+                effort -= C;
+            }
+
+            // stop motor if halted and stopped
+            if (abs(error) < 3. && targetSpeed == 0) {
+                effort = 0;
+            }
+
             // Set the effort for the motor
             SetEffort(effort);
 

src/robot.cpp

@@ -102,8 +102,9 @@ void Robot::LineFollowingUpdate(void)
     if(robotState == ROBOT_LINING) 
     {
         // TODO: calculate the error in CalcError(), calc the effort, and update the motion
-        int16_t lineError = lineSensor.CalcError();
-        float turnEffort = 0;
+        float lineError = 3.5 - lineSensor.CalcError();
+        Serial.println(lineError);
+        float turnEffort = lineError * lining_kP;
 
         chassis.SetTwist(baseSpeed, turnEffort);
     }

src/robot.h

@@ -3,6 +3,8 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 
+static float lining_kP = 1.0;
+
 class Robot
 {
 protected: