David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: reckoner.cpp
- Revision:
- 46:f11cb4f93aac
- Parent:
- 37:23000a47ed2b
--- a/reckoner.cpp Wed Apr 15 19:19:19 2015 +0000 +++ b/reckoner.cpp Wed Apr 15 21:19:22 2015 +0000 @@ -1,5 +1,6 @@ #include <mbed.h> #include "reckoner.h" +#include "math.h" /** First, we define two int32_t variables cos and sin that make a unit vector. @@ -7,7 +8,7 @@ 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]. +The starting value of the [cos, sin] will be [1 << 30, 0]. To record this choice, we define LOG_UNIT_MAGNITUDE: **/ #define LOG_UNIT_MAGNITUDE 30 @@ -109,8 +110,8 @@ void Reckoner::reset() { - cos = 1 << LOG_UNIT_MAGNITUDE; - sin = 0; + cosv = 1 << LOG_UNIT_MAGNITUDE; + sinv = 0; x = 0; y = 0; } @@ -141,14 +142,14 @@ void Reckoner::handleForward() { - x += cos >> LOG_COS_TO_X_CONVERSION; - y += sin >> LOG_COS_TO_X_CONVERSION; + x += cosv >> LOG_COS_TO_X_CONVERSION; + y += sinv >> LOG_COS_TO_X_CONVERSION; } void Reckoner::handleBackward() { - x -= cos >> LOG_COS_TO_X_CONVERSION; - y -= sin >> LOG_COS_TO_X_CONVERSION; + x -= cosv >> LOG_COS_TO_X_CONVERSION; + y -= sinv >> LOG_COS_TO_X_CONVERSION; } void Reckoner::handleRight() @@ -165,10 +166,18 @@ void Reckoner::handleTurnRadians(int32_t radians) { - 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; + int32_t dc = -((int64_t)sinv * radians) >> LOG_UNIT_MAGNITUDE; + int32_t ds = ((int64_t)cosv * radians) >> LOG_UNIT_MAGNITUDE; + dc = -((int64_t)(sinv+ds/2) * radians) >> LOG_UNIT_MAGNITUDE; + ds = ((int64_t)(cosv+dc/2) * radians) >> LOG_UNIT_MAGNITUDE; + cosv += dc; + sinv += ds; +} + +// For angle, 0x20000000 represents 45 degrees. +void Reckoner::setTurnAngle(int32_t angle) +{ + double angleToRadiansFactor = 1.46291808e-9; // pi/4 / 0x20000000 + cosv = (int32_t)(cos(angle * angleToRadiansFactor) * (1 << LOG_UNIT_MAGNITUDE)); + sinv = (int32_t)(sin(angle * angleToRadiansFactor) * (1 << LOG_UNIT_MAGNITUDE)); } \ No newline at end of file