lib/LineSensor/src/LineSensor.cpp

@@ -2,9 +2,6 @@
 
 #define DARK_THRESHOLD 500;
 
-#define LINE_THRESHOLD 200;
-#define INTERSECTION_THRESHOLD 500;
-
 void LineSensor::Initialize(void)
 {
     for (int i = 0; i<sensorCount; i++) {
@@ -12,20 +9,6 @@ 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,12 +12,9 @@ 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,40 +97,16 @@ void Robot::EnterLineFollowing(float speed)
     robotState = ROBOT_LINING;
 }
 
-int lineLostFrames = 7;
-
 void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
     {
-        float setpoint = 3.5;
+        // 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;
 
-        float lineError = setpoint - lineSensor.CalcError();
+        chassis.SetTwist(baseSpeed, turnEffort);
-
-        float powErr = (lineError*lineError);
-        powErr = (lineError < 0 ? -powErr : powErr);
-
-        float turnEffort = powErr * lining_kP;
-
-        rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);
-
-        float speed = baseSpeed;
-
-        speed *= 1 - (abs(rollingTurnRate) * KTurnRate);
-
-        if (!lineSensor.LineDetected()) { 
-            lineLostFrames -= 1;
-            if (lineLostFrames < 0) {
-                rollingTurnRate = 0; 
-                turnEffort = 0; 
-                speed = 0;
-            }
-        }
-        else { 
-            lineLostFrames = 7;
-        }
-
-        chassis.SetTwist(speed, turnEffort);
     }
 }
 

src/robot.h

@@ -3,7 +3,7 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 
-static float lining_kP = 1.24;
+static float lining_kP = 1.0;
 
 class Robot
 {
@@ -57,10 +57,6 @@ protected:
     /* baseSpeed is used to drive at a given speed while, say, line following.*/
     float baseSpeed = 0;
 
-    float rollingTurnRate = 0;
-    float turnRateDamp = 0.96; // the proportion of the turn rate retained each loop
-    float KTurnRate = 0.25; // slowdown coefficient
-
     /**
      * For tracking the motion of the Romi. We keep track of the intersection we came
      * from and the one we're headed to. You'll program in the map in handleIntersection()