David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 50:517c0f0e621f
- Parent:
- 49:eaa6fd514f4f
- Child:
- 51:b9f7243609d4
--- a/main.cpp Wed Apr 15 21:58:21 2015 +0000 +++ b/main.cpp Wed Apr 15 22:53:33 2015 +0000 @@ -74,8 +74,9 @@ setLeds(1, 0, 0, 0); waitForSignalToStart(); - setLeds(0, 1, 0, 0); - followLineFast(); + setLeds(0, 1, 0, 0); // led4 gets set when it detects the start + //followLineFast(); + followLineSmart(); setLeds(1, 1, 1, 1); loggerReportLoop(); @@ -284,22 +285,15 @@ Pacer reportPacer(200000); loadCalibration(); - uint32_t loopCount = 0; Timer timer; timer.start(); turnSensor.start(); while(1) { - loopCount += 1; turnSensor.update(); updateReckoner(turnSensor); loggerService(); - if ((loopCount % 256) == 0) - { - pc.printf("%d\r\n", lineTracker.getLinePosition()); - } - lineTracker.read(); updateMotorsToFollowLineFast(); @@ -311,3 +305,44 @@ motorsSpeedSet(0, 0); } +bool foundStart() +{ + static int16_t lastX = 0; + return lastX < 0 && reckoner.x >= 0 && abs(reckoner.y) < (85 << 16) && + totalEncoderCounts > 10000 && abs(turnSensor.getAngle()) < turnAngle1 * 30; + lastX = reckoner.x; +} + +void followLineSmart() +{ + totalEncoderCounts = 0; + Pacer reportPacer(200000); + + loadCalibration(); + turnSensor.start(); + while(1) + { + turnSensor.update(); + updateReckoner(turnSensor); + loggerService(); + + lineTracker.read(); + updateMotorsToFollowLineFast(); + + if (foundStart()) + { + reckoner.reset(); + turnSensor.reset(); + totalEncoderCounts = 0; + nextLogEncoderCount = 0; + led4 = 1; + } + + if (button1DefinitelyPressed()) + { + break; + } + } + motorsSpeedSet(0, 0); +} +