tuned constants

notably updated wheel diameter to 7cm

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 * 4.0);
+const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 7);
-const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
+const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 7);
 
 /**
  * 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 = 20;
+const uint16_t CONTROL_LOOP_PERIOD_MS = 12;
 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 200;
+#define LINE_THRESHOLD 110;
 #define INTERSECTION_THRESHOLD 500;
 
 void LineSensor::Initialize(void)
@@ -49,15 +49,5 @@ float LineSensor::CalcError(void)
 
 bool LineSensor::CheckIntersection(void)
 {
-    bool retVal = false;
+    return AverageReflectance() > INTERSECTION_THRESHOLD;
-
+}
-    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 = 30.;
+    float TRAPEZOIDAL_RAMP_RATE_CM_S = 10.;
     // 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,20 +97,43 @@ void Robot::EnterLineFollowing(float speed)
     robotState = ROBOT_LINING;
 }
 
-int lineLostFrames = 7;
+int lineLostFrames = 24;
+
+float lastError = 0;
 
 void Robot::LineFollowingUpdate(void)
 {
     if(robotState == ROBOT_LINING) 
     {
-        float setpoint = 3.5;
+        float setpoint = rollingTurnRate > 0.1 ? 4 : 3;
 
         float lineError = setpoint - lineSensor.CalcError();
 
+
         float powErr = (lineError*lineError);
         powErr = (lineError < 0 ? -powErr : powErr);
 
-        float turnEffort = powErr * lining_kP;
+        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
 
         rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);
 
@@ -118,16 +141,25 @@ 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 = 7;
+            lineLostFrames = 24;
         }
 
         chassis.SetTwist(speed, turnEffort);

src/robot.h

@@ -3,7 +3,9 @@
 #include <LineSensor.h>
 #include <LSM6.h>
 
-static float lining_kP = 1.24;
+static float lining_kP2 = 0.67;
+static float lining_kP = 0.72;
+static float lining_kD = -1.2;
 
 class Robot
 {
@@ -58,8 +60,8 @@ protected:
     float baseSpeed = 0;
 
     float rollingTurnRate = 0;
-    float turnRateDamp = 0.96; // the proportion of the turn rate retained each loop
+    float turnRateDamp = 0.975; // the proportion of the turn rate retained each loop
-    float KTurnRate = 0.25; // slowdown coefficient
+    float KTurnRate = 0.18; // slowdown coefficient
 
     /**
      * For tracking the motion of the Romi. We keep track of the intersection we came