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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers reckoner.cpp Source File

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 }