David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 40:e79cefc241f8
- Parent:
- 39:a5e25fd52ff8
- Child:
- 41:4a4be80ff38c
diff -r a5e25fd52ff8 -r e79cefc241f8 main.cpp --- a/main.cpp Tue Apr 14 01:06:41 2015 +0000 +++ b/main.cpp Tue Apr 14 01:45:01 2015 +0000 @@ -58,7 +58,7 @@ setLeds(1, 0, 0, 0); waitForSignalToStart(); - setLeds(0, 1, 0, 0); + setLeds(0, 1, 0, 0); followLineFast(); setLeds(1, 1, 1, 1); @@ -181,7 +181,7 @@ void updateMotorsToFollowLine() { const int16_t drivingSpeed = 400; - const int followLineStrength = drivingSpeed * 5 / 4; + const int32_t followLineStrength = drivingSpeed * 5 / 4; int16_t speedLeft = drivingSpeed; int16_t speedRight = drivingSpeed; @@ -201,21 +201,21 @@ void updateMotorsToFollowLineFast() { const int16_t drivingSpeed = 600; - const int followLineStrength = drivingSpeed * 5 / 4; - static int16_t lastPosition = 0; + const int32_t followLineStrength = drivingSpeed * 5 / 4; + static int16_t lastPosition = 1000; int16_t position = lineTracker.getLinePosition(); int16_t speedLeft = drivingSpeed; int16_t speedRight = drivingSpeed; - int16_t reduction = (position - 1000) * followLineStrength / 1000; - if(reduction < 0) + int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200; + if(veer > 0) { - speedLeft = reduceSpeed(speedLeft, -reduction); + speedRight = reduceSpeed(speedRight, veer); } else { - speedRight = reduceSpeed(speedRight, reduction); + speedLeft = reduceSpeed(speedLeft, -veer); } motorsSpeedSet(speedLeft, speedRight); @@ -228,11 +228,20 @@ Pacer reportPacer(200000); loadCalibration(); + uint32_t loopCount = 0; + Timer timer; + timer.start(); while(1) { + loopCount += 1; updateReckonerFromEncoders(); loggerService(); - + + if ((loopCount % 256) == 0) + { + pc.printf("%d\r\n", lineTracker.getLinePosition()); + } + lineTracker.read(); updateMotorsToFollowLineFast(); }