David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 52:05a8e919ddb0
- Parent:
- 51:b9f7243609d4
- Child:
- 53:73565a3ef5c5
--- a/main.cpp Wed Apr 15 23:01:39 2015 +0000 +++ b/main.cpp Wed Apr 15 23:56:52 2015 +0000 @@ -21,6 +21,7 @@ Logger logger; Pacer loggerPacer(50000); +uint8_t lapsCompleted = 0; uint32_t totalEncoderCounts = 0; uint32_t nextLogEncoderCount = 0; const uint32_t logSpacing = 100; @@ -73,8 +74,8 @@ setLeds(1, 0, 0, 0); waitForSignalToStart(); - - setLeds(0, 1, 0, 0); // led4 gets set when it detects the start + + setLeds(0, 1, 0, 0); // led3 and led4 get set by followLineSmart for debugging //followLineFast(); followLineSmart(); @@ -234,7 +235,7 @@ wait(0.2); } -void updateMotorsToFollowLine() +void updateMotorsToFollowLineSlow() { const int16_t drivingSpeed = 400; const int32_t followLineStrength = drivingSpeed * 5 / 4; @@ -254,9 +255,8 @@ motorsSpeedSet(speedLeft, speedRight); } -void updateMotorsToFollowLineFast() +void updateMotorsToFollowLineFast(int16_t drivingSpeed) { - const int16_t drivingSpeed = 1000; const int32_t followLineStrength = drivingSpeed * 5 / 4; static int16_t lastPosition = 1000; @@ -295,7 +295,7 @@ loggerService(); lineTracker.read(); - updateMotorsToFollowLineFast(); + updateMotorsToFollowLineFast(1000); if (button1DefinitelyPressed()) { @@ -314,9 +314,59 @@ return result; } +bool onLongStraightPart() +{ + if (lapsCompleted == 0) { return false; } + + // Figure out what part of the log corresponds to our current situation. + uint32_t logIndex = totalEncoderCounts / logSpacing; + + if (logIndex >= logger.getSize()) + { + // Should not happen. + return false; + } + + // To improve this, we could check that turnSensor.getAngle() matches what is in the log. + + uint32_t angle1 = turnSensor.getAngleUnsigned(); + + // 1000 encoder ticks + const uint32_t lookAheadAmount = 2000 / logSpacing; + + // Figure out how far away the next turn is. + uint32_t i = logIndex; + while(1) + { + i++; + + if (i >= logger.getSize()) + { + // reached the end the log + return false; + } + + if (i > logIndex + lookAheadAmount) + { + // looked far enough ahead that we don't think there is a turn coming up soon + return true; + } + + + uint32_t angle2 = (uint16_t)logger.entries[i].turnAngle << 16; + if (abs((int32_t)(angle2 - angle1)) > turnAngle45) + { + // detected a turn + return false; + } + } +} + void followLineSmart() { + lapsCompleted = 0; totalEncoderCounts = 0; + Pacer reportPacer(200000); loadCalibration(); @@ -328,16 +378,42 @@ loggerService(); lineTracker.read(); - updateMotorsToFollowLineFast(); + + // By default, choose a cautious speed of 1000 (out of 1200). + int16_t speed = 1000; + + // Go fast if we know we are on a long straight part. + if (onLongStraightPart()) + { + speed = 1200; + led3 = 1; + } + else + { + led3 = 0; + } + + + updateMotorsToFollowLineFast(speed); if (foundStart()) { + // Another lap has been completed! + lapsCompleted = 1; + led4 = lapsCompleted & 1; + reckoner.reset(); turnSensor.reset(); totalEncoderCounts = 0; nextLogEncoderCount = 0; - led4 = 1; - } + } + + if (lapsCompleted == 3 && totalEncoderCounts > 2000) + { + // Classy move: know when you are done with the competition and stop automatically. + // (Of course, there is a risk that this will backfire!) + break; + } if (button1DefinitelyPressed()) {