File content as of revision 57:99bec7fab454:

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

/**
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 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:                         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*35/(30*12) / 150 = 0.00439471394
**/

/** The theoretical value for dA above turned out to not be so accurate.
After playing with the robot for a few minutes, the robot was confused about which direction
it was facing by about 30 degrees.
So I did an experiment to find out what dA really is.  I turned the robot around many times and
then took the difference in the encoder ticks from the left and right wheels.
dA should be equal to 2*pi*(turn count) / (left_ticks - right_ticks).
The experiment was performed several times to see how accurate the number is.

(Theoretical dA                       = 0.00439471394 )
Run 1: dA = 2*pi*15 / (11691 + 9414)  = 0.00446566119
Run 2: dA = 2*pi*15 / (10232 + 10961) = 0.00444711836
Run 3: dA = 2*pi*30 / (19823 + 22435) = 0.00446058874
Run 4: dA = 2*pi*30 / (19794 + 22447) = 0.00446238392

The dA I decided to use is the average from runs 3 and 4:  dA = 0.00446148633
**/

#define DA 4790484  // 0.00446148633 * 0x40000000

#define LOG_COS_TO_X_CONVERSION  16    // 30 - 14

Reckoner::Reckoner()
{
    reset();
}

void Reckoner::reset()
{
  cosv = 1 << LOG_UNIT_MAGNITUDE;
  sinv = 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 += cosv >> LOG_COS_TO_X_CONVERSION;
    y += sinv >> LOG_COS_TO_X_CONVERSION;
}

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

void Reckoner::handleRight()
{
    // DA = 4790484
    // 0.2% boost
    handleTurnRadians(-4800065);
}

void Reckoner::handleLeft()
{
    handleTurnRadians(DA);
}

void Reckoner::handleTurnRadians(int32_t radians)
{
    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));
}