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@43:0e985a58f174, 2019-07-27 (annotated)

Committer:
DavidEGrayson
Date:
Sat Jul 27 22:52:19 2019 +0000
Revision:
43:0e985a58f174
Parent:
37:23000a47ed2b 
Changed reckoner to use readings from turnSensor (Gyro) to get its direction vector instead of encoder ticks.

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 }