David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 24:fc01d9125d3b
- Parent:
- 23:aae5cbe3b924
- Child:
- 25:73c2eedb3b91
diff -r aae5cbe3b924 -r fc01d9125d3b main.cpp --- a/main.cpp Fri Feb 28 00:23:00 2014 +0000 +++ b/main.cpp Fri Feb 28 01:07:14 2014 +0000 @@ -145,11 +145,68 @@ reckoner.reset(); } +// Keep the robot pointing the in the right direction (1, 0). +// This should basically keep it going straight. +void updateMotorsToDriveStraight() +{ + const int32_t straightDriveStrength = 1000; + int16_t speedLeft = drivingSpeed; + int16_t speedRight = drivingSpeed; + int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15); + if (reduction > 0) + { + speedRight = reduceSpeed(speedRight, reduction); + } + else + { + speedLeft = reduceSpeed(speedLeft, -reduction); + } + motorsSpeedSet(speedLeft, speedRight); +} + +void loadCalibrationAndFindLine() +{ + lineTracker.calibratedMinimum[0] = 34872; + lineTracker.calibratedMinimum[1] = 29335; + lineTracker.calibratedMinimum[2] = 23845; + lineTracker.calibratedMaximum[0] = 59726; + lineTracker.calibratedMaximum[1] = 60110; + lineTracker.calibratedMaximum[2] = 58446; + + Timer foundLineTimer; + foundLineTimer.start(); + bool foundLine = false; + + GeneralDebouncer lineStatus(10000); + while(1) + { + lineTracker.read(); + lineTracker.updateCalibration(); + updateReckonerFromEncoders(); + updateMotorsToDriveStraight(); + lineStatus.update(lineTracker.getLineVisible()); + + if (foundLine) + { + if(foundLineTimer.read_ms() >= 500) + { + // We found the line and traveled for a bit more, so now we can be done. + break; + } + } + else + { + if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 150000) + { + foundLine = true; + foundLineTimer.start(); + } + } + } +} void findLineAndCalibrate() { - const int32_t straightDriveStrength = 1000; - Timer timer; timer.start(); @@ -160,21 +217,8 @@ { lineTracker.read(); lineTracker.updateCalibration(); - updateReckonerFromEncoders(); - - // Keep the robot pointing the in the right direction. This should basically keep it going straight. - int16_t speedLeft = drivingSpeed; - int16_t speedRight = drivingSpeed; - int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15); - if (reduction > 0) - { - speedRight = reduceSpeed(speedRight, reduction); - } - else - { - speedLeft = reduceSpeed(speedLeft, -reduction); - } - motorsSpeedSet(speedLeft, speedRight); + updateReckonerFromEncoders(); + updateMotorsToDriveStraight(); if (goodCalibration) {