David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 28:4374035df5e0
- Parent:
- 27:2456f68be679
- Child:
- 29:cfcf08d8ac79
--- a/main.cpp Sat Mar 01 01:46:35 2014 +0000 +++ b/main.cpp Sat Mar 01 03:13:57 2014 +0000 @@ -44,13 +44,17 @@ //testDriveHome(); //testFinalSettleIn(); //testCalibrate(); + //testLineFollowing(); + testAnalog(); // Real routines for the contest. + loadCalibration(); + setLeds(1, 0, 0, 0); waitForSignalToStart(); setLeds(0, 1, 0, 0); - loadCalibrationAndFindLine(); + findLine(); //findLineAndCalibrate(); //setLeds(1, 1, 0, 0); @@ -65,6 +69,16 @@ while(1){} } +void loadCalibration() +{ + lineTracker.calibratedMinimum[0] = 34872; + lineTracker.calibratedMinimum[1] = 29335; + lineTracker.calibratedMinimum[2] = 23845; + lineTracker.calibratedMaximum[0] = 59726; + lineTracker.calibratedMaximum[1] = 60110; + lineTracker.calibratedMaximum[2] = 58446; +} + void updateReckonerFromEncoders() { while(encoderBuffer.hasEvents()) @@ -167,15 +181,27 @@ motorsSpeedSet(speedLeft, speedRight); } -void loadCalibrationAndFindLine() +void updateMotorsToFollowLine() { - lineTracker.calibratedMinimum[0] = 34872; - lineTracker.calibratedMinimum[1] = 29335; - lineTracker.calibratedMinimum[2] = 23845; - lineTracker.calibratedMaximum[0] = 59726; - lineTracker.calibratedMaximum[1] = 60110; - lineTracker.calibratedMaximum[2] = 58446; + const int followLineStrength = 300; + + int16_t speedLeft = drivingSpeed; + int16_t speedRight = drivingSpeed; + int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000; + if(reduction < 0) + { + speedLeft = reduceSpeed(speedLeft, -reduction); + } + else + { + speedRight = reduceSpeed(speedRight, reduction); + } + motorsSpeedSet(speedLeft, speedRight); +} + +void findLine() +{ GeneralDebouncer lineStatus(10000); while(1) { @@ -252,7 +278,6 @@ GeneralDebouncer lineStatus(10000); const uint32_t lineDebounceTime = 100000; - const int followLineStrength = 300; while(1) { @@ -263,25 +288,12 @@ bool lostLine = lineStatus.getState() == false && lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime; - if(lostLine && timer.read_us() >= 300000) + if(lostLine && timer.read_us() >= 2000000) { break; } - int16_t speedLeft = drivingSpeed; - int16_t speedRight = drivingSpeed; - int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500; - if(reduction < 0) - { - speedLeft = reduceSpeed(speedLeft, -reduction); - } - else - { - speedRight = reduceSpeed(speedRight, reduction); - } - - - motorsSpeedSet(speedLeft, speedRight); + updateMotorsToFollowLine(); } }