David Grayson / Mbed 2 deprecated DeadReckoning

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

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?

UserRevisionLine numberNew 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 }