File content as of revision 48:597738b77f77:

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

// 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()
{
    reset();
}

void Reckoner::reset()
{
    cosv = 1 << RECKONER_LOG_UNIT;
    sinv = 0;
    x = 0;
    y = 0;
}

void Reckoner::handleForward()
{
    x += cosv;
    y += sinv;
}

void Reckoner::handleBackward()
{
    x -= cosv;
    y -= sinv;
}

void Reckoner::setTurnAngle(int32_t angle)
{
    // 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));
}