David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
Revision 18:b65fbb795396, committed 2014-02-24
- Comitter:
- DavidEGrayson
- Date:
- Mon Feb 24 02:32:59 2014 +0000
- Parent:
- 17:2df9861f53ee
- Child:
- 19:a11ffc903774
- Commit message:
- Got the robot to face towards home!
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
test.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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) + { + + } +}
--- a/test.cpp Mon Feb 24 01:38:55 2014 +0000 +++ b/test.cpp Mon Feb 24 02:32:59 2014 +0000 @@ -18,7 +18,7 @@ void testDriveHome() { led1 = 1; - while(button1DefinitelyPressed()) + while(!button1DefinitelyPressed()) { updateReckonerFromEncoders(); }