David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 21:c279c6a83671
- Parent:
- 20:dbec34f0e76b
- Child:
- 22:44c032e59ff5
diff -r dbec34f0e76b -r c279c6a83671 main.cpp --- a/main.cpp Thu Feb 27 19:46:35 2014 +0000 +++ b/main.cpp Thu Feb 27 23:20:34 2014 +0000 @@ -1,7 +1,9 @@ #include <mbed.h> #include <Pacer.h> +#include <GeneralDebouncer.h> #include <math.h> +#include "main.h" #include "motors.h" #include "encoders.h" #include "leds.h" @@ -9,6 +11,20 @@ #include "test.h" #include "reckoner.h" #include "buttons.h" +#include "line_tracker.h" + +Reckoner reckoner; +LineTracker lineTracker; + +const int16_t drivingSpeed = 300; + +void setLeds(bool v1, bool v2, bool v3, bool v4) +{ + led1 = v1; + led2 = v2; + led3 = v3; + led4 = v4; +} int __attribute__((noreturn)) main() { @@ -26,12 +42,23 @@ //testReckoner(); //testButtons(); //testDriveHome(); - testFinalSettleIn(); + //testFinalSettleIn(); - while(1) - { - - } + // Real routines for the contest. + setLeds(1, 0, 0, 0); + waitForSignalToStart(); + setLeds(0, 1, 0, 0); + findLineAndCalibrate(); + setLeds(1, 1, 0, 0); + turnRightToFindLine(); + setLeds(0, 0, 1, 0); + followLineToEnd(); + setLeds(1, 0, 1, 0); + driveHomeAlmost(); + setLeds(0, 1, 1, 0); + finalSettleIn(); + setLeds(1, 1, 1, 1); + while(1){} } void updateReckonerFromEncoders() @@ -81,7 +108,7 @@ return (reckoner.x * s - reckoner.y * c) / magnitude(); } -int16_t reduceSpeed(int16_t speed, int16_t reduction) +int16_t reduceSpeed(int16_t speed, int32_t reduction) { if (reduction > speed) { @@ -93,26 +120,146 @@ } } -void driveHomeAlmost(); -void finalSettleIn(); +// Returns true if each line sensor has one third of a volt of difference between the +// maximum seen value and the minimum. +bool calibrationLooksGood() +{ + for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) + { + uint16_t contrast = lineTracker.calibratedMaximum[s] - lineTracker.calibratedMinimum[s]; + if (contrast < 9929) // 0xFFFF*0.5/3.3 = 9929 + { + return false; + } + } + return true; +} + +void waitForSignalToStart() +{ + while(!button1DefinitelyPressed()) + { + updateReckonerFromEncoders(); + } + reckoner.reset(); +} + -void driveHome() +void findLineAndCalibrate() { - driveHomeAlmost(); - finalSettleIn(); + const int32_t straightDriveStrength = 1000; + + Timer timer; + timer.start(); + + Timer goodCalibrationTimer; + bool goodCalibration = false; + + while(1) + { + 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); + + if (goodCalibration) + { + if(goodCalibrationTimer.read_us() >= 300000) + { + // The calibration was good and we traveled for a bit of time after that, + // so we must be a bit over the line. + break; + } + } + else + { + if(calibrationLooksGood()) + { + goodCalibration = true; + goodCalibrationTimer.start(); + } + } + } +} + +void turnRightToFindLine() +{ + while(1) + { + lineTracker.read(); + lineTracker.updateCalibration(); + updateReckonerFromEncoders(); + + if(lineTracker.getLineVisible()) + { + break; + } + + motorsSpeedSet(300, 100); + } +} + +void followLineToEnd() +{ + GeneralDebouncer lineStatus(10000); + const uint32_t lineDebounceTime = 100000; + const int followLineStrength = 300; + + while(1) + { + lineTracker.read(); + updateReckonerFromEncoders(); + + lineStatus.update(lineTracker.getLineVisible()); + + bool lostLine = lineStatus.getState() == false && + lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime; + + if(lostLine) + { + break; + } + + if(lineTracker.getLineVisible()) + { + break; + } + + int16_t speedLeft = drivingSpeed; + int16_t speedRight = drivingSpeed; + int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500; + if(reduction < 0) + { + reduceSpeed(speedLeft, -reduction); + } + else + { + reduceSpeed(speedRight, reduction); + } + + + motorsSpeedSet(speedLeft, speedRight); + } } void driveHomeAlmost() { - led1 = 1; led2 = 1; led3 = 0; led4 = 0; - - bool dir = false; - uint16_t transitions = 0; Timer timer; timer.start(); - const int16_t maxSpeed = 300; - while(1) { updateReckonerFromEncoders(); @@ -127,17 +274,11 @@ float det = determinant(); - { - bool nextDir = det > 0; - if (nextDir != dir) { transitions++; } - dir = nextDir; - } - - int16_t speedLeft = maxSpeed; - int16_t speedRight = maxSpeed; + int16_t speedLeft = drivingSpeed; + int16_t speedRight = drivingSpeed; if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down. { - int16_t reduction = (1 - magn/(1<<20)) * maxSpeed; + int16_t reduction = (1 - magn/(1<<20)) * drivingSpeed; speedLeft = reduceSpeed(speedLeft, reduction); speedRight = reduceSpeed(speedRight, reduction); } @@ -158,8 +299,6 @@ void finalSettleIn() { - led1 = 1; led2 = 1; led3 = 1; led4 = 0; - const int16_t settleSpeed = 200; const int16_t settleModificationStrength = 100; @@ -170,6 +309,8 @@ // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0]. uint8_t state = 0; + Pacer reportPacer(200000); + while(1) { updateReckonerFromEncoders(); @@ -185,17 +326,23 @@ speedModification = -settleModificationStrength; } - if (state == 0 && timer.read_ms() >= 6000 && reckoner.cos > (1 << 29)) + if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29)) { - led1 = 1; led2 = 0; led3 = 1; led4 = 0; + // Stop turning and start trying to maintain the right position. state = 1; } + if (state == 1 && timer.read_ms() >= 5000) + { + // Stop moving. + break; + } + int16_t rotationSpeed; if (state == 1) { float s = (float)reckoner.sin / (1 << 30); - rotationSpeed = -s * 300; + rotationSpeed = -s * 600; } else { @@ -205,8 +352,15 @@ 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()); + } } - led1 = 1; led2 = 1; led3 = 1; led4 = 1; + // Done! Stop moving. motorsSpeedSet(0, 0); }