David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 39:a5e25fd52ff8
- Parent:
- 38:5e93a479c244
- Child:
- 40:e79cefc241f8
--- a/main.cpp Thu Mar 13 17:49:43 2014 +0000 +++ b/main.cpp Tue Apr 14 01:06:41 2015 +0000 @@ -18,8 +18,6 @@ Logger logger; Pacer loggerPacer(50000); -const int16_t drivingSpeed = 400; - void setLeds(bool v1, bool v2, bool v3, bool v4) { led1 = v1; @@ -59,21 +57,9 @@ setLeds(1, 0, 0, 0); waitForSignalToStart(); - + setLeds(0, 1, 0, 0); - findLine(); - - //setLeds(1, 1, 0, 0); - //turnRightToFindLine(); - - setLeds(0, 0, 1, 0); - followLineToEnd(); - - setLeds(1, 0, 1, 0); - driveHomeAlmost(); - - //setLeds(0, 1, 1, 0); - //finalSettleIn(); + followLineFast(); setLeds(1, 1, 1, 1); loggerReportLoop(); @@ -102,11 +88,11 @@ void loadCalibration() { /** QTR-3RC **/ - lineTracker.calibratedMinimum[0] = 100; - lineTracker.calibratedMinimum[1] = 94; - lineTracker.calibratedMinimum[2] = 103; - lineTracker.calibratedMaximum[0] = 792; - lineTracker.calibratedMaximum[1] = 807; + lineTracker.calibratedMinimum[0] = 137; + lineTracker.calibratedMinimum[1] = 132; + lineTracker.calibratedMinimum[2] = 154; + lineTracker.calibratedMaximum[0] = 644; + lineTracker.calibratedMaximum[1] = 779; lineTracker.calibratedMaximum[2] = 1000; /** QTR-3A @@ -192,27 +178,9 @@ wait(0.2); } -// 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 updateMotorsToFollowLine() { + const int16_t drivingSpeed = 400; const int followLineStrength = drivingSpeed * 5 / 4; int16_t speedLeft = drivingSpeed; @@ -227,201 +195,46 @@ speedRight = reduceSpeed(speedRight, reduction); } - motorsSpeedSet(speedLeft, speedRight); -} - -void findLine() -{ - GeneralDebouncer lineStatus(10000); - while(1) - { - lineTracker.read(); - lineTracker.updateCalibration(); - updateReckonerFromEncoders(); - loggerService(); - updateMotorsToDriveStraight(); - lineStatus.update(lineTracker.getLineVisible()); - - if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 20000) - { - break; - } - } + motorsSpeedSet(speedLeft, speedRight); } -/** -void turnRightToFindLine() -{ - while(1) - { - lineTracker.read(); - lineTracker.updateCalibration(); - updateReckonerFromEncoders(); - - if(lineTracker.getLineVisible()) - { - break; - } - - motorsSpeedSet(300, 100); - } -}**/ - -void followLineToEnd() +void updateMotorsToFollowLineFast() { - Timer timer; - timer.start(); - - GeneralDebouncer lineStatus(10000); - const uint32_t lineDebounceTime = 1000000; - - while(1) - { - lineTracker.read(); - updateReckonerFromEncoders(); - loggerService(); + const int16_t drivingSpeed = 600; + const int followLineStrength = drivingSpeed * 5 / 4; + static int16_t lastPosition = 0; + + int16_t position = lineTracker.getLinePosition(); - lineStatus.update(lineTracker.getLineVisible()); - - bool lostLine = lineStatus.getState() == false && - lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime; - if(lostLine && timer.read_us() >= 2000000) - { - break; - } - - updateMotorsToFollowLine(); + int16_t speedLeft = drivingSpeed; + int16_t speedRight = drivingSpeed; + int16_t reduction = (position - 1000) * followLineStrength / 1000; + if(reduction < 0) + { + speedLeft = reduceSpeed(speedLeft, -reduction); } + else + { + speedRight = reduceSpeed(speedRight, reduction); + } + + motorsSpeedSet(speedLeft, speedRight); + + lastPosition = position; } -void driveHomeAlmost() -{ - Timer timer; - timer.start(); +void followLineFast() +{ + Pacer reportPacer(200000); + loadCalibration(); while(1) { updateReckonerFromEncoders(); loggerService(); - float magn = magnitude(); - - if (magn < (1<<(14+7))) - { - // We are within 128 encoder ticks, so go to the next step. - break; - } - - float det = determinant(); - - int16_t speedLeft = drivingSpeed; - int16_t speedRight = drivingSpeed; - if (magn < (1<<(14+9))) // Within 512 encoder ticks of the origin, so slow down. - { - int16_t reduction = (1 - magn/(1<<(14+8))) * (drivingSpeed/2); - speedLeft = reduceSpeed(speedLeft, reduction); - speedRight = reduceSpeed(speedRight, reduction); - } - - if (det > 0) - { - speedLeft = reduceSpeed(speedLeft, det * 1000); - } - else - { - speedRight = reduceSpeed(speedRight, -det * 1000); - } - motorsSpeedSet(speedLeft, speedRight); + lineTracker.read(); + updateMotorsToFollowLineFast(); } - - motorsSpeedSet(0, 0); } -void finalSettleIn() -{ - const int16_t settleSpeed = 300; - const int16_t settleModificationStrength = 150; - - Timer timer; - timer.start(); - - // State 0: rotating - // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0]. - uint8_t state = 0; - - Pacer reportPacer(200000); - Pacer motorUpdatePacer(10000); - - float integral = 0; - - motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling - - while(1) - { - led1 = (state == 1); - - updateReckonerFromEncoders(); - loggerService(); - - float dot = dotProduct(); - int16_t speedModification = -dot * settleModificationStrength; - if (speedModification > settleModificationStrength) - { - speedModification = settleModificationStrength; - } - else if (speedModification < -settleModificationStrength) - { - speedModification = -settleModificationStrength; - } - - if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29)) - { - // Stop turning and start trying to maintain the right position. - state = 1; - } - - if (state == 1 && timer.read_ms() >= 5000) - { - // Stop moving. - break; - } - - if (motorUpdatePacer.pace()) - { - int16_t rotationSpeed; - if (state == 1) - { - float s = (float)reckoner.sin / (1 << 30); - integral += s; - rotationSpeed = -(s * 2400 + integral * 20); - - if (rotationSpeed > 300) - { - rotationSpeed = 300; - } - if (rotationSpeed < -300) - { - rotationSpeed = -300; - } - } - else - { - rotationSpeed = settleSpeed; - } - - int16_t speedLeft = -rotationSpeed + speedModification; - int16_t speedRight = rotationSpeed + speedModification; - motorsSpeedSet(speedLeft, speedRight); - } - - if (state == 1 && reportPacer.pace()) - { - pc.printf("%11d %11d %11d %11d | %11f %11f\r\n", - reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, - determinant(), dotProduct()); - } - } - - // Done! Stop moving. - motorsSpeedSet(0, 0); -}