David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
reckoner.cpp@12:835a4d24ae3b, 2014-02-23 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Sun Feb 23 22:23:34 2014 +0000
- Revision:
- 12:835a4d24ae3b
- Child:
- 13:bba5b3abd13f
Made the Reckoner class and wrote a routine to help test it.;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 12:835a4d24ae3b | 1 | #include <mbed.h> |
DavidEGrayson | 12:835a4d24ae3b | 2 | #include "reckoner.h" |
DavidEGrayson | 12:835a4d24ae3b | 3 | |
DavidEGrayson | 12:835a4d24ae3b | 4 | /** |
DavidEGrayson | 12:835a4d24ae3b | 5 | W: Wheel-to-wheel distance: 150 mm // TODO: correct this |
DavidEGrayson | 12:835a4d24ae3b | 6 | G: Gear ratio factor: 30 |
DavidEGrayson | 12:835a4d24ae3b | 7 | T: Encoder ticks per backshaft rotation: 12 (three-toothed encoder wheel) |
DavidEGrayson | 12:835a4d24ae3b | 8 | R: Wheel radius: 70 mm |
DavidEGrayson | 12:835a4d24ae3b | 9 | |
DavidEGrayson | 12:835a4d24ae3b | 10 | Dw: Distance wheel turns per encoder tick = 2*pi*R/(G*T) |
DavidEGrayson | 12:835a4d24ae3b | 11 | Df: Distance robot moves forward per encoder tick = Dw/2 |
DavidEGrayson | 12:835a4d24ae3b | 12 | dA: Change in angle per encoder tick = Dw/W = 2*pi*70/(30*12) / 150 = 0.00814486984 |
DavidEGrayson | 12:835a4d24ae3b | 13 | **/ |
DavidEGrayson | 12:835a4d24ae3b | 14 | |
DavidEGrayson | 12:835a4d24ae3b | 15 | #define LOG_UNIT_MAGNITUDE 30 |
DavidEGrayson | 12:835a4d24ae3b | 16 | |
DavidEGrayson | 12:835a4d24ae3b | 17 | #define DA 8745487 // 0.00814486984 * 0x40000000 |
DavidEGrayson | 12:835a4d24ae3b | 18 | |
DavidEGrayson | 12:835a4d24ae3b | 19 | #define LOG_COS_TO_X_CONVERSION 14 // 30 - 16 |
DavidEGrayson | 12:835a4d24ae3b | 20 | |
DavidEGrayson | 12:835a4d24ae3b | 21 | Reckoner reckoner; |
DavidEGrayson | 12:835a4d24ae3b | 22 | |
DavidEGrayson | 12:835a4d24ae3b | 23 | Reckoner::Reckoner() |
DavidEGrayson | 12:835a4d24ae3b | 24 | { |
DavidEGrayson | 12:835a4d24ae3b | 25 | cos = 1 << LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 26 | sin = 0; |
DavidEGrayson | 12:835a4d24ae3b | 27 | x = 0; |
DavidEGrayson | 12:835a4d24ae3b | 28 | y = 0; |
DavidEGrayson | 12:835a4d24ae3b | 29 | } |
DavidEGrayson | 12:835a4d24ae3b | 30 | |
DavidEGrayson | 12:835a4d24ae3b | 31 | void Reckoner::handleTickLeftForward() |
DavidEGrayson | 12:835a4d24ae3b | 32 | { |
DavidEGrayson | 12:835a4d24ae3b | 33 | handleForward(); |
DavidEGrayson | 12:835a4d24ae3b | 34 | handleRight(); |
DavidEGrayson | 12:835a4d24ae3b | 35 | } |
DavidEGrayson | 12:835a4d24ae3b | 36 | |
DavidEGrayson | 12:835a4d24ae3b | 37 | void Reckoner::handleTickLeftBackward() |
DavidEGrayson | 12:835a4d24ae3b | 38 | { |
DavidEGrayson | 12:835a4d24ae3b | 39 | handleBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 40 | handleLeft(); |
DavidEGrayson | 12:835a4d24ae3b | 41 | } |
DavidEGrayson | 12:835a4d24ae3b | 42 | |
DavidEGrayson | 12:835a4d24ae3b | 43 | void Reckoner::handleTickRightForward() |
DavidEGrayson | 12:835a4d24ae3b | 44 | { |
DavidEGrayson | 12:835a4d24ae3b | 45 | handleForward(); |
DavidEGrayson | 12:835a4d24ae3b | 46 | handleLeft(); |
DavidEGrayson | 12:835a4d24ae3b | 47 | } |
DavidEGrayson | 12:835a4d24ae3b | 48 | |
DavidEGrayson | 12:835a4d24ae3b | 49 | void Reckoner::handleTickRightBackward() |
DavidEGrayson | 12:835a4d24ae3b | 50 | { |
DavidEGrayson | 12:835a4d24ae3b | 51 | handleBackward(); |
DavidEGrayson | 12:835a4d24ae3b | 52 | handleRight(); |
DavidEGrayson | 12:835a4d24ae3b | 53 | } |
DavidEGrayson | 12:835a4d24ae3b | 54 | |
DavidEGrayson | 12:835a4d24ae3b | 55 | void Reckoner::handleForward() |
DavidEGrayson | 12:835a4d24ae3b | 56 | { |
DavidEGrayson | 12:835a4d24ae3b | 57 | x += cos >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 58 | y += sin >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 59 | } |
DavidEGrayson | 12:835a4d24ae3b | 60 | |
DavidEGrayson | 12:835a4d24ae3b | 61 | void Reckoner::handleBackward() |
DavidEGrayson | 12:835a4d24ae3b | 62 | { |
DavidEGrayson | 12:835a4d24ae3b | 63 | x -= cos >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 64 | y -= sin >> LOG_COS_TO_X_CONVERSION; |
DavidEGrayson | 12:835a4d24ae3b | 65 | } |
DavidEGrayson | 12:835a4d24ae3b | 66 | |
DavidEGrayson | 12:835a4d24ae3b | 67 | void Reckoner::handleRight() |
DavidEGrayson | 12:835a4d24ae3b | 68 | { |
DavidEGrayson | 12:835a4d24ae3b | 69 | cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 70 | sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 71 | } |
DavidEGrayson | 12:835a4d24ae3b | 72 | |
DavidEGrayson | 12:835a4d24ae3b | 73 | void Reckoner::handleLeft() |
DavidEGrayson | 12:835a4d24ae3b | 74 | { |
DavidEGrayson | 12:835a4d24ae3b | 75 | cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 76 | sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; |
DavidEGrayson | 12:835a4d24ae3b | 77 | } |