lib/LineSensor/src/LineSensor.cpp

@@ -4,29 +4,13 @@
 
 void LineSensor::Initialize(void)
 {
-    for (int i = 0; i<sensorCount; i++) {
-        pinMode(sensors[i], INPUT);
-    }
+    pinMode(leftSensorPin, INPUT);
+    pinMode(rightSensorPin, INPUT);
 }
 
-float LineSensor::CalcError(void) 
+int16_t LineSensor::CalcError(void) 
 { 
-    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;
+    return analogRead(rightSensorPin) - analogRead(leftSensorPin); 
 }
     
 

lib/LineSensor/src/LineSensor.h

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

lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h

@@ -24,18 +24,15 @@ protected:
     enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
     volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
 
-    float Kp = 2.50; // proportional to error
-    float Ki = 0.03;
+    // TODO: After you tune your motors, set the gains here.
+    float Kp = 1;
+    float Ki = 0;
     float Kd = 0;
-    float Kf = 3.00; // proportional to setpoint
-    int16_t C = 22; // constant added to effort
+    float Kf = 0;
 
     // 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.
@@ -128,11 +125,6 @@ protected:
 
             sumError += error; 
 
-            float maxSumError = maxEffort/Ki;
-
-            sumError = max(sumError, -maxSumError);
-            sumError = min(sumError, maxSumError);
-
             float errorDiff = error - prevError;
 
             prevError = error;
@@ -140,18 +132,6 @@ 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,9 +102,8 @@ void Robot::LineFollowingUpdate(void)
     if(robotState == ROBOT_LINING) 
     {
         // TODO: calculate the error in CalcError(), calc the effort, and update the motion
-        float lineError = 3.5 - lineSensor.CalcError();
-        Serial.println(lineError);
-        float turnEffort = lineError * lining_kP;
+        int16_t lineError = lineSensor.CalcError();
+        float turnEffort = 0;
 
         chassis.SetTwist(baseSpeed, turnEffort);
     }

src/robot.h

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