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@48:597738b77f77, 2019-08-13 (annotated)

Committer:
DavidEGrayson
Date:
Tue Aug 13 21:21:17 2019 +0000
Revision:
48:597738b77f77
Parent:
43:0e985a58f174 
Changes from before the contest, I think.

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 43:0e985a58f174 4 // We define Df to be the distance the robot moves forward per encoder tick.
DavidEGrayson 43:0e985a58f174 5 // This is half of the amount a single wheel turns per encoder tick.
DavidEGrayson 43:0e985a58f174 6 // Df is NOT an integer so we cannot store it in our program; it is a distance.
DavidEGrayson 43:0e985a58f174 7 // We do not need to even know Df, because all of our distances can be
DavidEGrayson 43:0e985a58f174 8 // measured and recorded in terms of Df.
DavidEGrayson 43:0e985a58f174 9 //
DavidEGrayson 43:0e985a58f174 10 // We define two int32_t variables x and y to hold the current position of the
DavidEGrayson 43:0e985a58f174 11 // robot. Together, x and y are a vector that points from the starting point to
DavidEGrayson 43:0e985a58f174 12 // the robot's current position.
DavidEGrayson 43:0e985a58f174 13 //
DavidEGrayson 43:0e985a58f174 14 // We choose the units of x and y to be Df / (1 << 14).
DavidEGrayson 43:0e985a58f174 15 //
DavidEGrayson 43:0e985a58f174 16 // Let's make sure this won't result in overflow:
DavidEGrayson 43:0e985a58f174 17 // To ensure no overflow happens, we need:
DavidEGrayson 43:0e985a58f174 18 // (Maximum distance representable by x or y) > (Maximum roam of robot)
DavidEGrayson 43:0e985a58f174 19 // (1 << 31) * Df / (1 << 14) > (Maximum roam of robot)
DavidEGrayson 43:0e985a58f174 20 // (Maximum roam of robot) / Df < (1 << 17) = 131072
DavidEGrayson 43:0e985a58f174 21 //
DavidEGrayson 43:0e985a58f174 22 // If we assume Df is 0.1 mm (which is pretty small), then this means the robot
DavidEGrayson 43:0e985a58f174 23 // can roam at most 13.1 m (0.0001 m * 131072) from its starting point,
DavidEGrayson 43:0e985a58f174 24 // which should be plenty.
DavidEGrayson 43:0e985a58f174 25 //
DavidEGrayson 43:0e985a58f174 26 // So how do we update x and y?
DavidEGrayson 43:0e985a58f174 27 // We define two int32_t variables named cosv and sinv that are in the same
DavidEGrayson 43:0e985a58f174 28 // units as x and y and represent a vector that points in the direction that
DavidEGrayson 43:0e985a58f174 29 // the robot is pointing and has a magnitude of Df (i.e. 1 << 14).
DavidEGrayson 43:0e985a58f174 30 // So we just do x += cosv and y += sinv to update x and y when the robot
DavidEGrayson 43:0e985a58f174 31 // moves one encoder tick forward.
DavidEGrayson 12:835a4d24ae3b 32
DavidEGrayson 21:c279c6a83671 33 Reckoner::Reckoner()
DavidEGrayson 21:c279c6a83671 34 {
DavidEGrayson 21:c279c6a83671 35 reset();
DavidEGrayson 21:c279c6a83671 36 }
DavidEGrayson 12:835a4d24ae3b 37
DavidEGrayson 21:c279c6a83671 38 void Reckoner::reset()
DavidEGrayson 12:835a4d24ae3b 39 {
DavidEGrayson 43:0e985a58f174 40 cosv = 1 << RECKONER_LOG_UNIT;
DavidEGrayson 43:0e985a58f174 41 sinv = 0;
DavidEGrayson 43:0e985a58f174 42 x = 0;
DavidEGrayson 43:0e985a58f174 43 y = 0;
DavidEGrayson 12:835a4d24ae3b 44 }
DavidEGrayson 12:835a4d24ae3b 45
DavidEGrayson 12:835a4d24ae3b 46 void Reckoner::handleForward()
DavidEGrayson 12:835a4d24ae3b 47 {
DavidEGrayson 43:0e985a58f174 48 x += cosv;
DavidEGrayson 43:0e985a58f174 49 y += sinv;
DavidEGrayson 12:835a4d24ae3b 50 }
DavidEGrayson 12:835a4d24ae3b 51
DavidEGrayson 12:835a4d24ae3b 52 void Reckoner::handleBackward()
DavidEGrayson 12:835a4d24ae3b 53 {
DavidEGrayson 43:0e985a58f174 54 x -= cosv;
DavidEGrayson 43:0e985a58f174 55 y -= sinv;
DavidEGrayson 12:835a4d24ae3b 56 }
DavidEGrayson 12:835a4d24ae3b 57
DavidEGrayson 43:0e985a58f174 58 void Reckoner::setTurnAngle(int32_t angle)
DavidEGrayson 36:ccb03b734737 59 {
DavidEGrayson 43:0e985a58f174 60 // 0x20000000 represents 45 degrees.
DavidEGrayson 43:0e985a58f174 61 const double angleToRadiansFactor = 1.46291808e-9; // pi/4 / 0x20000000
DavidEGrayson 43:0e985a58f174 62 cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT));
DavidEGrayson 43:0e985a58f174 63 sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT));
DavidEGrayson 43:0e985a58f174 64 }