David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 27:2456f68be679
- Parent:
- 26:7e7c376a7446
- Child:
- 28:4374035df5e0
--- 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()) {