File content as of revision 13:bba5b3abd13f:

#include <mbed.h>
#include "reckoner.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 ofvalue 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
**/

#define DA 4718788  // 0.00439471394 * 0x40000000

#define LOG_COS_TO_X_CONVERSION  16    // 30 - 14

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;
}