David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: reckoner.cpp
- Revision:
- 13:bba5b3abd13f
- Parent:
- 12:835a4d24ae3b
- Child:
- 14:c8cca3687e64
diff -r 835a4d24ae3b -r bba5b3abd13f reckoner.cpp --- a/reckoner.cpp Sun Feb 23 22:23:34 2014 +0000 +++ b/reckoner.cpp Sun Feb 23 23:49:58 2014 +0000 @@ -2,21 +2,88 @@ #include "reckoner.h" /** -W: Wheel-to-wheel distance: 150 mm // TODO: correct this +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: 70 mm +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*70/(30*12) / 150 = 0.00814486984 +dA: Change in angle per encoder tick = Dw/W = 2*pi*35/(30*12) / 150 = 0.00439471394 **/ -#define LOG_UNIT_MAGNITUDE 30 +#define DA 4718788 // 0.00439471394 * 0x40000000 -#define DA 8745487 // 0.00814486984 * 0x40000000 - -#define LOG_COS_TO_X_CONVERSION 14 // 30 - 16 +#define LOG_COS_TO_X_CONVERSION 16 // 30 - 14 Reckoner reckoner;