David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
main.cpp@18:b65fbb795396, 2014-02-24 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Mon Feb 24 02:32:59 2014 +0000
- Revision:
- 18:b65fbb795396
- Parent:
- 17:2df9861f53ee
- Child:
- 19:a11ffc903774
Got the robot to face towards home!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 0:e77a0edb9878 | 1 | #include <mbed.h> |
DavidEGrayson | 8:78b1ff957cba | 2 | #include <Pacer.h> |
DavidEGrayson | 0:e77a0edb9878 | 3 | |
DavidEGrayson | 8:78b1ff957cba | 4 | #include "motors.h" |
DavidEGrayson | 8:78b1ff957cba | 5 | #include "encoders.h" |
DavidEGrayson | 9:9734347b5756 | 6 | #include "leds.h" |
DavidEGrayson | 8:78b1ff957cba | 7 | #include "pc_serial.h" |
DavidEGrayson | 9:9734347b5756 | 8 | #include "test.h" |
DavidEGrayson | 12:835a4d24ae3b | 9 | #include "reckoner.h" |
DavidEGrayson | 16:8eaa5bc2bdb1 | 10 | #include "buttons.h" |
DavidEGrayson | 0:e77a0edb9878 | 11 | |
DavidEGrayson | 10:e4dd36148539 | 12 | int __attribute__((noreturn)) main() |
DavidEGrayson | 2:968338353aef | 13 | { |
DavidEGrayson | 2:968338353aef | 14 | pc.baud(115200); |
DavidEGrayson | 2:968338353aef | 15 | |
DavidEGrayson | 2:968338353aef | 16 | // Enable pull-ups on encoder pins and give them a chance to settle. |
DavidEGrayson | 9:9734347b5756 | 17 | encodersInit(); |
DavidEGrayson | 9:9734347b5756 | 18 | motorsInit(); |
DavidEGrayson | 16:8eaa5bc2bdb1 | 19 | buttonsInit(); |
DavidEGrayson | 4:1b20a11765c8 | 20 | |
DavidEGrayson | 8:78b1ff957cba | 21 | // Test routines |
DavidEGrayson | 9:9734347b5756 | 22 | //testMotors(); |
DavidEGrayson | 10:e4dd36148539 | 23 | //testEncoders(); |
DavidEGrayson | 12:835a4d24ae3b | 24 | //testLineSensors(); |
DavidEGrayson | 16:8eaa5bc2bdb1 | 25 | //testReckoner(); |
DavidEGrayson | 17:2df9861f53ee | 26 | //testButtons(); |
DavidEGrayson | 17:2df9861f53ee | 27 | testDriveHome(); |
DavidEGrayson | 2:968338353aef | 28 | |
DavidEGrayson | 4:1b20a11765c8 | 29 | while(1) |
DavidEGrayson | 4:1b20a11765c8 | 30 | { |
DavidEGrayson | 9:9734347b5756 | 31 | |
DavidEGrayson | 0:e77a0edb9878 | 32 | } |
DavidEGrayson | 0:e77a0edb9878 | 33 | } |
DavidEGrayson | 12:835a4d24ae3b | 34 | |
DavidEGrayson | 12:835a4d24ae3b | 35 | void updateReckonerFromEncoders() |
DavidEGrayson | 12:835a4d24ae3b | 36 | { |
DavidEGrayson | 12:835a4d24ae3b | 37 | while(encoderBuffer.hasEvents()) |
DavidEGrayson | 12:835a4d24ae3b | 38 | { |
DavidEGrayson | 12:835a4d24ae3b | 39 | PololuEncoderEvent event = encoderBuffer.readEvent(); |
DavidEGrayson | 12:835a4d24ae3b | 40 | switch(event) |
DavidEGrayson | 12:835a4d24ae3b | 41 | { |
DavidEGrayson | 17:2df9861f53ee | 42 | case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC: |
DavidEGrayson | 17:2df9861f53ee | 43 | reckoner.handleTickLeftForward(); |
DavidEGrayson | 17:2df9861f53ee | 44 | break; |
DavidEGrayson | 17:2df9861f53ee | 45 | case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC: |
DavidEGrayson | 17:2df9861f53ee | 46 | reckoner.handleTickLeftBackward(); |
DavidEGrayson | 17:2df9861f53ee | 47 | break; |
DavidEGrayson | 17:2df9861f53ee | 48 | case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC: |
DavidEGrayson | 17:2df9861f53ee | 49 | reckoner.handleTickRightForward(); |
DavidEGrayson | 17:2df9861f53ee | 50 | break; |
DavidEGrayson | 17:2df9861f53ee | 51 | case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC: |
DavidEGrayson | 17:2df9861f53ee | 52 | reckoner.handleTickRightBackward(); |
DavidEGrayson | 17:2df9861f53ee | 53 | break; |
DavidEGrayson | 12:835a4d24ae3b | 54 | } |
DavidEGrayson | 12:835a4d24ae3b | 55 | } |
DavidEGrayson | 12:835a4d24ae3b | 56 | } |
DavidEGrayson | 17:2df9861f53ee | 57 | |
DavidEGrayson | 18:b65fbb795396 | 58 | // The closer this is to zero, the closer we are to pointing towards the home position. |
DavidEGrayson | 18:b65fbb795396 | 59 | // It is basically a cross product of the two vectors (x, y) and (cos, sin). |
DavidEGrayson | 18:b65fbb795396 | 60 | float det() |
DavidEGrayson | 18:b65fbb795396 | 61 | { |
DavidEGrayson | 18:b65fbb795396 | 62 | // TODO: get rid of the magic numbers here (i.e. 30) |
DavidEGrayson | 18:b65fbb795396 | 63 | float s = (float)reckoner.sin / (1 << 30); |
DavidEGrayson | 18:b65fbb795396 | 64 | float c = (float)reckoner.cos / (1 << 30); |
DavidEGrayson | 18:b65fbb795396 | 65 | return reckoner.x * s - reckoner.y * c; |
DavidEGrayson | 18:b65fbb795396 | 66 | } |
DavidEGrayson | 18:b65fbb795396 | 67 | |
DavidEGrayson | 18:b65fbb795396 | 68 | void __attribute__((noreturn)) driveHome() |
DavidEGrayson | 18:b65fbb795396 | 69 | { |
DavidEGrayson | 18:b65fbb795396 | 70 | led1 = 1; led2 = 1; led3 = 0; led4 = 0; |
DavidEGrayson | 18:b65fbb795396 | 71 | |
DavidEGrayson | 18:b65fbb795396 | 72 | // First, point the robot at the goal. |
DavidEGrayson | 18:b65fbb795396 | 73 | bool dir = false; |
DavidEGrayson | 18:b65fbb795396 | 74 | uint16_t transitions = 0; |
DavidEGrayson | 18:b65fbb795396 | 75 | Timer timer; |
DavidEGrayson | 18:b65fbb795396 | 76 | timer.start(); |
DavidEGrayson | 18:b65fbb795396 | 77 | while(transitions < 100 || timer.read_ms() < 500) |
DavidEGrayson | 18:b65fbb795396 | 78 | { |
DavidEGrayson | 18:b65fbb795396 | 79 | updateReckonerFromEncoders(); |
DavidEGrayson | 18:b65fbb795396 | 80 | { |
DavidEGrayson | 18:b65fbb795396 | 81 | bool nextDir = det() > 0; |
DavidEGrayson | 18:b65fbb795396 | 82 | if (nextDir != dir) { transitions++; } |
DavidEGrayson | 18:b65fbb795396 | 83 | dir = nextDir; |
DavidEGrayson | 18:b65fbb795396 | 84 | } |
DavidEGrayson | 18:b65fbb795396 | 85 | |
DavidEGrayson | 18:b65fbb795396 | 86 | if(dir) |
DavidEGrayson | 18:b65fbb795396 | 87 | { |
DavidEGrayson | 18:b65fbb795396 | 88 | led3 = 1; |
DavidEGrayson | 18:b65fbb795396 | 89 | motorsSpeedSet(-300, 300); |
DavidEGrayson | 18:b65fbb795396 | 90 | } |
DavidEGrayson | 18:b65fbb795396 | 91 | else |
DavidEGrayson | 18:b65fbb795396 | 92 | { |
DavidEGrayson | 18:b65fbb795396 | 93 | led3 = 0; |
DavidEGrayson | 18:b65fbb795396 | 94 | motorsSpeedSet(300, -300); |
DavidEGrayson | 18:b65fbb795396 | 95 | } |
DavidEGrayson | 18:b65fbb795396 | 96 | } |
DavidEGrayson | 18:b65fbb795396 | 97 | motorsSpeedSet(0, 0); |
DavidEGrayson | 18:b65fbb795396 | 98 | |
DavidEGrayson | 18:b65fbb795396 | 99 | while(1) |
DavidEGrayson | 18:b65fbb795396 | 100 | { |
DavidEGrayson | 18:b65fbb795396 | 101 | |
DavidEGrayson | 18:b65fbb795396 | 102 | } |
DavidEGrayson | 18:b65fbb795396 | 103 | } |