David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 20:dbec34f0e76b
- Parent:
- 19:a11ffc903774
- Child:
- 21:c279c6a83671
--- a/main.cpp Tue Feb 25 02:58:16 2014 +0000 +++ b/main.cpp Thu Feb 27 19:46:35 2014 +0000 @@ -25,7 +25,8 @@ //testLineSensors(); //testReckoner(); //testButtons(); - testDriveHome(); + //testDriveHome(); + testFinalSettleIn(); while(1) { @@ -61,6 +62,15 @@ return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y); } +float dotProduct() +{ + float s = (float)reckoner.sin / (1 << 30); + float c = (float)reckoner.cos / (1 << 30); + float magn = magnitude(); + if (magn == 0){ return 0; } + return ((float)reckoner.x * c + (float)reckoner.y * s) / magn; +} + // The closer this is to zero, the closer we are to pointing towards the home position. // It is basically a cross product of the two vectors (x, y) and (cos, sin). float determinant() @@ -83,11 +93,19 @@ } } -void __attribute__((noreturn)) driveHome() +void driveHomeAlmost(); +void finalSettleIn(); + +void driveHome() +{ + driveHomeAlmost(); + finalSettleIn(); +} + +void driveHomeAlmost() { led1 = 1; led2 = 1; led3 = 0; led4 = 0; - // First, point the robot at the goal. bool dir = false; uint16_t transitions = 0; Timer timer; @@ -135,22 +153,60 @@ motorsSpeedSet(speedLeft, speedRight); } - //while(1) - { - - } + motorsSpeedSet(0, 0); +} + +void finalSettleIn() +{ + led1 = 1; led2 = 1; led3 = 1; led4 = 0; - motorsSpeedSet(0, 0); - Pacer reportPacer(200000); + const int16_t settleSpeed = 200; + const int16_t settleModificationStrength = 100; + + 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; + while(1) { - if(reportPacer.pace()) + updateReckonerFromEncoders(); + + 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() >= 6000 && reckoner.cos > (1 << 29)) { - led4 = 1; - pc.printf("%11d %11d %11d %11d | %11f\r\n", - reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, - determinant()); - led4 = 0; - } + led1 = 1; led2 = 0; led3 = 1; led4 = 0; + state = 1; + } + + int16_t rotationSpeed; + if (state == 1) + { + float s = (float)reckoner.sin / (1 << 30); + rotationSpeed = -s * 300; + } + else + { + rotationSpeed = settleSpeed; + } + + int16_t speedLeft = -rotationSpeed + speedModification; + int16_t speedRight = rotationSpeed + speedModification; + motorsSpeedSet(speedLeft, speedRight); } + + led1 = 1; led2 = 1; led3 = 1; led4 = 1; + motorsSpeedSet(0, 0); }