David Grayson / Mbed 2 deprecated DeadReckoning

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

Committer:
DavidEGrayson
Date:
2014-02-23
Revision:
12:835a4d24ae3b
Child:
13:bba5b3abd13f

File content as of revision 12:835a4d24ae3b:

#include <mbed.h>
#include "reckoner.h"

/**
W: Wheel-to-wheel distance:              150 mm  // TODO: correct this
G: Gear ratio factor:                    30
T: Encoder ticks per backshaft rotation: 12       (three-toothed encoder wheel)
R: Wheel radius:                         70 mm

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
**/

#define LOG_UNIT_MAGNITUDE 30

#define DA 8745487  // 0.00814486984 * 0x40000000

#define LOG_COS_TO_X_CONVERSION  14    // 30 - 16

Reckoner reckoner;

Reckoner::Reckoner()
{
  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();   
}

void Reckoner::handleForward()
{
    x += cos >> LOG_COS_TO_X_CONVERSION;
    y += sin >> LOG_COS_TO_X_CONVERSION;
}

void Reckoner::handleBackward()
{
    x -= cos >> LOG_COS_TO_X_CONVERSION;
    y -= sin >> LOG_COS_TO_X_CONVERSION;
}

void Reckoner::handleRight()
{
    cos += ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
    sin -= ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
}

void Reckoner::handleLeft()
{
    cos -= ((int64_t)sin * DA) >> LOG_UNIT_MAGNITUDE;
    sin += ((int64_t)cos * DA) >> LOG_UNIT_MAGNITUDE;
}