slow down on turns

lib/LineSensor/src/LineSensor.cpp

@@ -2,6 +2,9 @@
 
 #define DARK_THRESHOLD 500;
 
+#define LINE_THRESHOLD 200;
+#define INTERSECTION_THRESHOLD 500;
+
 void LineSensor::Initialize(void)
 {
     for (int i = 0; i<sensorCount; i++) {
@@ -9,6 +12,20 @@ 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,9 +12,12 @@ 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,16 +97,40 @@ void Robot::EnterLineFollowing(float speed)
     robotState = ROBOT_LINING;
 }
 
+int lineLostFrames = 7;
+
 void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
     {
-        // TODO: calculate the error in CalcError(), calc the effort, and update the motion
+        float setpoint = 3.5;
-        float lineError = 3.5 - lineSensor.CalcError();
-        Serial.println(lineError);
-        float turnEffort = lineError * lining_kP;
 
-        chassis.SetTwist(baseSpeed, turnEffort);
+        float lineError = setpoint - lineSensor.CalcError();
+
+        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.0;
+static float lining_kP = 1.24;
 
 class Robot
 {
@@ -57,6 +57,10 @@ 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()