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.h@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 // Robot configuration:
DavidEGrayson 12:835a4d24ae3b 2
DavidEGrayson 12:835a4d24ae3b 3 // General purpose code:
DavidEGrayson 12:835a4d24ae3b 4
DavidEGrayson 12:835a4d24ae3b 5 class Reckoner
DavidEGrayson 12:835a4d24ae3b 6 {
DavidEGrayson 12:835a4d24ae3b 7 public:
DavidEGrayson 12:835a4d24ae3b 8
DavidEGrayson 12:835a4d24ae3b 9 Reckoner();
DavidEGrayson 12:835a4d24ae3b 10
DavidEGrayson 12:835a4d24ae3b 11 // Together, cos and sin form a vector with a magnitude close to 2^30 that indicates
DavidEGrayson 12:835a4d24ae3b 12 // the current position of the robot. By definition, when the reckoning starts,
DavidEGrayson 12:835a4d24ae3b 13 // sin is 0 and cos is 2^30.
DavidEGrayson 12:835a4d24ae3b 14 // cos corresponds to the x component of the orientation vector.
DavidEGrayson 12:835a4d24ae3b 15 // sin corresponds to the y component of the orientation vector.
DavidEGrayson 12:835a4d24ae3b 16 int32_t cos, sin;
DavidEGrayson 12:835a4d24ae3b 17
DavidEGrayson 12:835a4d24ae3b 18 // Together, x and y are a vector that points from the starting point to the
DavidEGrayson 12:835a4d24ae3b 19 // robot's current position.
DavidEGrayson 12:835a4d24ae3b 20 // Units:
DavidEGrayson 12:835a4d24ae3b 21 // * If the units are too big, precision is lost.
DavidEGrayson 12:835a4d24ae3b 22 // * If they are too small, the integers wil overflow.
DavidEGrayson 12:835a4d24ae3b 23 // * For convenience, they should be a power of 2 off from Df (Distance robot moves
DavidEGrayson 12:835a4d24ae3b 24 // forward per encoder tick), so we can just write x += cos >> some_constant.
DavidEGrayson 12:835a4d24ae3b 25 // * Worst case for overflow: our encoders give us a Df of 0.25mm and our robot moves
DavidEGrayson 12:835a4d24ae3b 26 // 5m away from the starting point. Therefore, x and y might need to hold a value
DavidEGrayson 12:835a4d24ae3b 27 // 20000 or -20000 without overflowing.
DavidEGrayson 12:835a4d24ae3b 28 // Overflow condition: x = (1<<31)
DavidEGrayson 12:835a4d24ae3b 29 // 20000*Df < max_dist_x = (1<<31) * U
DavidEGrayson 12:835a4d24ae3b 30 // U > Df * (20000) / (1<<31) = Df / (1<<16)
DavidEGrayson 12:835a4d24ae3b 31 // * Therefore the units we choose for x are Df / (1<<16).
DavidEGrayson 12:835a4d24ae3b 32 // * If we wrote x += cos it would mean the units are Df / (1<<30).
DavidEGrayson 12:835a4d24ae3b 33 // Instead, we write x += cos >> 14 so the units are correct.
DavidEGrayson 12:835a4d24ae3b 34 int32_t x, y;
DavidEGrayson 12:835a4d24ae3b 35
DavidEGrayson 12:835a4d24ae3b 36 void handleTickLeftForward();
DavidEGrayson 12:835a4d24ae3b 37 void handleTickLeftBackward();
DavidEGrayson 12:835a4d24ae3b 38 void handleTickRightForward();
DavidEGrayson 12:835a4d24ae3b 39 void handleTickRightBackward();
DavidEGrayson 12:835a4d24ae3b 40 void handleForward();
DavidEGrayson 12:835a4d24ae3b 41 void handleBackward();
DavidEGrayson 12:835a4d24ae3b 42 void handleRight();
DavidEGrayson 12:835a4d24ae3b 43 void handleLeft();
DavidEGrayson 12:835a4d24ae3b 44 };
DavidEGrayson 12:835a4d24ae3b 45
DavidEGrayson 12:835a4d24ae3b 46 extern Reckoner reckoner;