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
Diff: reckoner.cpp
- Revision:
- 43:0e985a58f174
- Parent:
- 37:23000a47ed2b
diff -r 96671b71aac5 -r 0e985a58f174 reckoner.cpp --- a/reckoner.cpp Sat Jul 27 20:58:46 2019 +0000 +++ b/reckoner.cpp Sat Jul 27 22:52:19 2019 +0000 @@ -1,106 +1,34 @@ #include <mbed.h> #include "reckoner.h" -/** -First, we define two int32_t variables cos and sin that make a unit vector. -These variables hold our current estimation of the direction the robot is pointed. -We need to choose units for them that allow for a lot of accuracy without overflowing. -We choose the units to be 1/(1 << 30) so that they will typically be about half of the -way between 0 and overflow. -The starting value ofvalue of the [cos, sin] will be [1 << 30, 0]. -To record this choice, we define LOG_UNIT_MAGNITUDE: -**/ -#define LOG_UNIT_MAGNITUDE 30 - -/** -We define dA to be the effect of a single encoder tick on the angle the robot is -facing, in radians. This can be calculated from physical parameters of the robot. -The value of dA will usually be less than 0.01. -What we want to do to update cos and sin for a left-turning encoder tick is: - - cos -= sin * dA (floating point arithmetic) - sin += cos * dA (floating point arithmetic) - -Instead of doing that we can do the equivalent using integer arithmetic. -We define DA to be dA times (1 << LOG_UNIT_MAGNITUDE). -Then we do: - - cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE; - sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE; -**/ - -/** -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. - -When choosing the units of x and y, we have several considerations: - * If the units are too big, precision is lost. - * If the units are too small, the integers will overflow. - * For convenience, the units should be a power of 2 off from Df (Distance robot moves - forward per encoder tick), so we can just write lines like: - x += cos >> some_constant - -Taking this into account, 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 - * Assume Df is 0.1 mm (this is smaller than it will usually be for the robots we work with). - * That 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? - * When we update x and y, the position they represent changes by Df. - * x and y represent a unit vector, but their units are 1 / (1 << LOG_UNIT_MAGNITUDE). - * If we wrote x += cos it would mean the units of x are Df / (1 << LOG_UNIT_MAGNITUDE). - (and x would overflow after 2 or 3 ticks). - * Instead, we write x += cos >> 16 so the units are correct. -**/ - - - -/** -W: Wheel-to-wheel distance: 139 mm // Measured with calipers. -G: Gear ratio factor: 30 -T: Encoder ticks per backshaft rotation: 12 (three-toothed encoder wheel) -R: Wheel radius: 35 mm (Pololu 70mm diameter wheel) - -Dw: Distance wheel turns per encoder tick = 2*pi*R/(G*T) -Df: Distance robot moves forward per encoder tick = Dw/2 -dA: Change in angle per encoder tick = Dw/W = 2*pi*35/(30*12) / 150 = 0.00439471394 -**/ - -/** The theoretical value for dA above turned out to not be so accurate. -After playing with the robot for a few minutes, the robot was confused about which direction -it was facing by about 30 degrees. -So I did an experiment to find out what dA really is. I turned the robot around many times and -then took the difference in the encoder ticks from the left and right wheels. -dA should be equal to 2*pi*(turn count) / (left_ticks - right_ticks). -The experiment was performed several times to see how accurate the number is. - -(Theoretical dA = 0.00439471394 ) -Run 1: dA = 2*pi*15 / (11691 + 9414) = 0.00446566119 -Run 2: dA = 2*pi*15 / (10232 + 10961) = 0.00444711836 -Run 3: dA = 2*pi*30 / (19823 + 22435) = 0.00446058874 -Run 4: dA = 2*pi*30 / (19794 + 22447) = 0.00446238392 - -The dA I decided to use is the average from runs 3 and 4: dA = 0.00446148633 -**/ - -#define DA 4790484 // 0.00446148633 * 0x40000000 - -#define LOG_COS_TO_X_CONVERSION 16 // 30 - 14 +// 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() { @@ -109,66 +37,28 @@ void Reckoner::reset() { - cos = 1 << LOG_UNIT_MAGNITUDE; - sin = 0; - x = 0; - y = 0; -} - -void Reckoner::handleTickLeftForward() -{ - handleForward(); - handleRight(); -} - -void Reckoner::handleTickLeftBackward() -{ - handleBackward(); - handleLeft(); -} - -void Reckoner::handleTickRightForward() -{ - handleForward(); - handleLeft(); -} - -void Reckoner::handleTickRightBackward() -{ - handleBackward(); - handleRight(); + cosv = 1 << RECKONER_LOG_UNIT; + sinv = 0; + x = 0; + y = 0; } void Reckoner::handleForward() { - x += cos >> LOG_COS_TO_X_CONVERSION; - y += sin >> LOG_COS_TO_X_CONVERSION; + x += cosv; + y += sinv; } void Reckoner::handleBackward() { - x -= cos >> LOG_COS_TO_X_CONVERSION; - y -= sin >> LOG_COS_TO_X_CONVERSION; -} - -void Reckoner::handleRight() -{ - // DA = 4790484 - // 0.2% boost - handleTurnRadians(-4800065); + x -= cosv; + y -= sinv; } -void Reckoner::handleLeft() -{ - handleTurnRadians(DA); -} - -void Reckoner::handleTurnRadians(int32_t radians) +void Reckoner::setTurnAngle(int32_t angle) { - int32_t dc = -((int64_t)sin * radians) >> LOG_UNIT_MAGNITUDE; - int32_t ds = ((int64_t)cos * radians) >> LOG_UNIT_MAGNITUDE; - dc = -((int64_t)(sin+ds/2) * radians) >> LOG_UNIT_MAGNITUDE; - ds = ((int64_t)(cos+dc/2) * radians) >> LOG_UNIT_MAGNITUDE; - cos += dc; - sin += ds; -} \ No newline at end of file + // 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)); +}