lib/Chassis/src/constants.h

@@ -7,12 +7,12 @@
  * tune them.
  */
 const float ROBOT_RADIUS = 14.7 / 2;
-const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 7);
-const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 7);
+const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
+const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
 
 /**
  * You can change the control loop period, but you should use multiples of 4 ms to 
  * avoid rounding errors.
  */
-const uint16_t CONTROL_LOOP_PERIOD_MS = 12;
+const uint16_t CONTROL_LOOP_PERIOD_MS = 20;
 const float CONTROL_LOOP_PERIOD_S = CONTROL_LOOP_PERIOD_MS / 1000.;

lib/LineSensor/src/LineSensor.cpp

@@ -2,7 +2,7 @@
 
 #define DARK_THRESHOLD 500;
 
-#define LINE_THRESHOLD 110;
+#define LINE_THRESHOLD 200;
 #define INTERSECTION_THRESHOLD 500;
 
 void LineSensor::Initialize(void)
@@ -49,5 +49,15 @@ float LineSensor::CalcError(void)
 
 bool LineSensor::CheckIntersection(void)
 {
-    return AverageReflectance() > INTERSECTION_THRESHOLD;
-}
+    bool retVal = false;
+
+    bool isLeftDark = analogRead(leftSensorPin) > DARK_THRESHOLD;
+    bool isRightDark = analogRead(rightSensorPin) > DARK_THRESHOLD;
+
+    bool onIntersection = isLeftDark && isRightDark;
+    if(onIntersection && !prevOnIntersection) retVal = true;
+
+    prevOnIntersection = onIntersection;
+
+    return retVal;
+}

lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h

@@ -34,7 +34,7 @@ protected:
     // Used to keep track of the target speed, in counts / interval.
     float targetSpeed = 0;
 
-    float TRAPEZOIDAL_RAMP_RATE_CM_S = 10.;
+    float TRAPEZOIDAL_RAMP_RATE_CM_S = 30.;
     // maximum accelration in counts/interval
     float TRAPEZOIDAL_RAMP_RATE = TRAPEZOIDAL_RAMP_RATE_CM_S / LEFT_TICKS_PER_CM / CONTROL_LOOP_PERIOD_S;
     float currentSetpoint = 0;

src/robot.cpp

@@ -97,43 +97,20 @@ void Robot::EnterLineFollowing(float speed)
     robotState = ROBOT_LINING;
 }
 
-int lineLostFrames = 24;
-
-float lastError = 0;
+int lineLostFrames = 7;
 
 void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
     {
-        float setpoint = rollingTurnRate > 0.1 ? 4 : 3;
+        float setpoint = 3.5;
 
         float lineError = setpoint - lineSensor.CalcError();
 
-
         float powErr = (lineError*lineError);
         powErr = (lineError < 0 ? -powErr : powErr);
 
-        float errDiff = lastError - lineError; 
-        if (lineSensor.LineDetected()) {
-            lastError = lineError;
-        } else {
-            errDiff = 0;
-            lineError = lastError > 0 ? 7:-7;
-        }
-
-        float turnEffort = powErr * lining_kP2 + lineError * lining_kP + errDiff * lining_kD;
-
-#ifdef __LINING_DEBUG__
-        Serial.print(lineSensor.CalcError());
-        Serial.print(' ');
-        Serial.print(lineError * lining_kP);
-        Serial.print(' ');
-        Serial.print(errDiff * lining_kD);
-        Serial.print(' ');
-        Serial.print(turnEffort);
-        Serial.print(' ');
-        Serial.println(lineSensor.AverageReflectance());
-#endif
+        float turnEffort = powErr * lining_kP;
 
         rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);
 
@@ -141,25 +118,16 @@ void Robot::LineFollowingUpdate(void)
 
         speed *= 1 - (abs(rollingTurnRate) * KTurnRate);
 
-        if (lineSensor.CheckIntersection()) {
-            turnEffort = 0;
-            speed *= 0.8;
-        }
-
         if (!lineSensor.LineDetected()) { 
             lineLostFrames -= 1;
             if (lineLostFrames < 0) {
                 rollingTurnRate = 0; 
                 turnEffort = 0; 
                 speed = 0;
-                chassis.Stop();
-                return;
-            } else {
-                speed *= 0.97;
             }
         }
         else { 
-            lineLostFrames = 24;
+            lineLostFrames = 7;
         }
 
         chassis.SetTwist(speed, turnEffort);

src/robot.h

@@ -3,9 +3,7 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 
-static float lining_kP2 = 0.67;
-static float lining_kP = 0.72;
-static float lining_kD = -1.2;
+static float lining_kP = 1.24;
 
 class Robot
 {
@@ -60,8 +58,8 @@ protected:
     float baseSpeed = 0;
 
     float rollingTurnRate = 0;
-    float turnRateDamp = 0.975; // the proportion of the turn rate retained each loop
-    float KTurnRate = 0.18; // slowdown coefficient
+    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