David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: main.cpp
- Revision:
- 18:b65fbb795396
- Parent:
- 17:2df9861f53ee
- Child:
- 19:a11ffc903774
diff -r 2df9861f53ee -r b65fbb795396 main.cpp --- a/main.cpp Mon Feb 24 01:38:55 2014 +0000 +++ b/main.cpp Mon Feb 24 02:32:59 2014 +0000 @@ -32,15 +32,6 @@ } } -void __attribute__((noreturn)) driveHome() -{ - led1 = 1; led2 = 1; led3 = 0; led4 = 0; - while(1) - { - - } -} - void updateReckonerFromEncoders() { while(encoderBuffer.hasEvents()) @@ -64,3 +55,49 @@ } } +// 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 det() +{ + // TODO: get rid of the magic numbers here (i.e. 30) + float s = (float)reckoner.sin / (1 << 30); + float c = (float)reckoner.cos / (1 << 30); + return reckoner.x * s - reckoner.y * c; +} + +void __attribute__((noreturn)) driveHome() +{ + led1 = 1; led2 = 1; led3 = 0; led4 = 0; + + // First, point the robot at the goal. + bool dir = false; + uint16_t transitions = 0; + Timer timer; + timer.start(); + while(transitions < 100 || timer.read_ms() < 500) + { + updateReckonerFromEncoders(); + { + bool nextDir = det() > 0; + if (nextDir != dir) { transitions++; } + dir = nextDir; + } + + if(dir) + { + led3 = 1; + motorsSpeedSet(-300, 300); + } + else + { + led3 = 0; + motorsSpeedSet(300, -300); + } + } + motorsSpeedSet(0, 0); + + while(1) + { + + } +}