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
00001 #include <mbed.h> 00002 #include "reckoner.h" 00003 00004 // We define Df to be the distance the robot moves forward per encoder tick. 00005 // This is half of the amount a single wheel turns per encoder tick. 00006 // Df is NOT an integer so we cannot store it in our program; it is a distance. 00007 // We do not need to even know Df, because all of our distances can be 00008 // measured and recorded in terms of Df. 00009 // 00010 // We define two int32_t variables x and y to hold the current position of the 00011 // robot. Together, x and y are a vector that points from the starting point to 00012 // the robot's current position. 00013 // 00014 // We choose the units of x and y to be Df / (1 << 14). 00015 // 00016 // Let's make sure this won't result in overflow: 00017 // To ensure no overflow happens, we need: 00018 // (Maximum distance representable by x or y) > (Maximum roam of robot) 00019 // (1 << 31) * Df / (1 << 14) > (Maximum roam of robot) 00020 // (Maximum roam of robot) / Df < (1 << 17) = 131072 00021 // 00022 // If we assume Df is 0.1 mm (which is pretty small), then this means the robot 00023 // can roam at most 13.1 m (0.0001 m * 131072) from its starting point, 00024 // which should be plenty. 00025 // 00026 // So how do we update x and y? 00027 // We define two int32_t variables named cosv and sinv that are in the same 00028 // units as x and y and represent a vector that points in the direction that 00029 // the robot is pointing and has a magnitude of Df (i.e. 1 << 14). 00030 // So we just do x += cosv and y += sinv to update x and y when the robot 00031 // moves one encoder tick forward. 00032 00033 Reckoner::Reckoner() 00034 { 00035 reset(); 00036 } 00037 00038 void Reckoner::reset() 00039 { 00040 cosv = 1 << RECKONER_LOG_UNIT; 00041 sinv = 0; 00042 x = 0; 00043 y = 0; 00044 } 00045 00046 void Reckoner::handleForward() 00047 { 00048 x += cosv; 00049 y += sinv; 00050 } 00051 00052 void Reckoner::handleBackward() 00053 { 00054 x -= cosv; 00055 y -= sinv; 00056 } 00057 00058 void Reckoner::setTurnAngle(int32_t angle) 00059 { 00060 // 0x20000000 represents 45 degrees. 00061 const double angleToRadiansFactor = 1.46291808e-9; // pi/4 / 0x20000000 00062 cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT)); 00063 sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << RECKONER_LOG_UNIT)); 00064 }
Generated on Sun Jul 17 2022 15:45:15 by 1.7.2