David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Revision 27:2456f68be679, committed 2014-03-01
- Comitter:
- DavidEGrayson
- Date:
- Sat Mar 01 01:46:35 2014 +0000
- Parent:
- 26:7e7c376a7446
- Child:
- 28:4374035df5e0
- Commit message:
- Fixed a major bug in the line following (reduceSpeed return value was not used). Made finalSettleIn better by adding an integral term and increasing the settleSpeed from 200 to 300.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Feb 28 01:40:39 2014 +0000 +++ b/main.cpp Sat Mar 01 01:46:35 2014 +0000 @@ -53,8 +53,8 @@ loadCalibrationAndFindLine(); //findLineAndCalibrate(); - setLeds(1, 1, 0, 0); - turnRightToFindLine(); + //setLeds(1, 1, 0, 0); + //turnRightToFindLine(); setLeds(0, 0, 1, 0); followLineToEnd(); setLeds(1, 0, 1, 0); @@ -273,11 +273,11 @@ int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500; if(reduction < 0) { - reduceSpeed(speedLeft, -reduction); + speedLeft = reduceSpeed(speedLeft, -reduction); } else { - reduceSpeed(speedRight, reduction); + speedRight = reduceSpeed(speedRight, reduction); } @@ -329,8 +329,8 @@ void finalSettleIn() { - const int16_t settleSpeed = 200; - const int16_t settleModificationStrength = 100; + const int16_t settleSpeed = 300; + const int16_t settleModificationStrength = 150; Timer timer; timer.start(); @@ -339,10 +339,17 @@ // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0]. uint8_t state = 0; - Pacer reportPacer(200000); + 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(); float dot = dotProduct(); @@ -368,20 +375,33 @@ break; } - int16_t rotationSpeed; - if (state == 1) + if (motorUpdatePacer.pace()) { - float s = (float)reckoner.sin / (1 << 30); - rotationSpeed = -s * 600; + 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); } - else - { - rotationSpeed = settleSpeed; - } - - int16_t speedLeft = -rotationSpeed + speedModification; - int16_t speedRight = rotationSpeed + speedModification; - motorsSpeedSet(speedLeft, speedRight); if (state == 1 && reportPacer.pace()) {