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
- Committer:
- DavidEGrayson
- Date:
- 2019-07-27
- Revision:
- 43:0e985a58f174
- Parent:
- 37:23000a47ed2b
File content as of revision 43:0e985a58f174:
#include <mbed.h> #include "reckoner.h" // We define Df to be the distance the robot moves forward per encoder tick. // This is half of the amount a single wheel turns per encoder tick. // Df is NOT an integer so we cannot store it in our program; it is a distance. // We do not need to even know Df, because all of our distances can be // measured and recorded in terms of Df. // // We define two int32_t variables x and y to hold the current position of the // robot. Together, x and y are a vector that points from the starting point to // the robot's current position. // // We choose the units of x and y to be Df / (1 << 14). // // Let's make sure this won't result in overflow: // To ensure no overflow happens, we need: // (Maximum distance representable by x or y) > (Maximum roam of robot) // (1 << 31) * Df / (1 << 14) > (Maximum roam of robot) // (Maximum roam of robot) / Df < (1 << 17) = 131072 // // If we assume Df is 0.1 mm (which is pretty small), then this means the robot // can roam at most 13.1 m (0.0001 m * 131072) from its starting point, // which should be plenty. // // So how do we update x and y? // We define two int32_t variables named cosv and sinv that are in the same // units as x and y and represent a vector that points in the direction that // the robot is pointing and has a magnitude of Df (i.e. 1 << 14). // So we just do x += cosv and y += sinv to update x and y when the robot // moves one encoder tick forward. Reckoner::Reckoner() { reset(); } void Reckoner::reset() { cosv = 1 << RECKONER_LOG_UNIT; sinv = 0; x = 0; y = 0; } void Reckoner::handleForward() { x += cosv; y += sinv; } void Reckoner::handleBackward() { x -= cosv; y -= sinv; } void Reckoner::setTurnAngle(int32_t angle) { // 0x20000000 represents 45 degrees. const double angleToRadiansFactor = 1.46291808e-9; // pi/4 / 0x20000000 cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT)); sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT)); }