File content as of revision 2:1255feea1c59:

#include "Navigate_Pololu.h"
#include "mbed.h"
#include <cstring>
#include "m3pi.h"

Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth,PinName robotTx, PinName robotRx):leftOpto(leftIn),rightOpto(rightIn),robot(D7,robotTx,robotRx){
    calibratePos(); //call routine to calibrate wheels
    X = 0.0f; //set x coordinate to zero
    Y = 0.0f; //set y coordinate to zero
    heading = 0.0f; //set heading to zero
    leftCounts = 0; //reset counter for left wheel
    rightCounts = 0; //reset counter for right wheel
    previousLeftCounts = 0;
    previousRightCounts = 0;
    distancePerCount = (PI * wheelDiameter) / (float)countsPerRevolution; //set variable
    radiansPerCount = PI * (wheelDiameter/trackWidth)/countsPerRevolution; //set variable
    leftOpto.rise(this, &Navigate_Pololu::leftWheelInterrupt); //create interrupt for left opto switch
    rightOpto.rise(this, &Navigate_Pololu::rightWheelInterrupt); //create interrupt for right opto switch
    printf("DistancePerCount: %f, RadiansPerCounts: %f\n",distancePerCount,radiansPerCount); //print out initial setup variables
}

void Navigate_Pololu::leftWheelInterrupt(){
    if(robot.leftDir == 1)
        leftCounts++;
    else
        leftCounts--;
    UpdatePosition(); //recalculate position
    previousLeftCounts = leftCounts;
}

void Navigate_Pololu::rightWheelInterrupt(){
    if(robot.rightDir == 1)
        rightCounts++;
    else
        rightCounts--;
    UpdatePosition(); //recalculate position
    previousRightCounts = rightCounts;
}

void Navigate_Pololu::UpdatePosition(){
    deltaLeft = leftCounts - previousLeftCounts;
    deltaRight = rightCounts - previousRightCounts;
    deltaDistance = 0.5f * (float)(deltaLeft + deltaRight) * distancePerCount;
    deltaHeading = (float) (deltaRight - deltaLeft) * radiansPerCount;
    heading += deltaHeading; //add deltaHeading to running total
    deltaX = deltaDistance * (float)cos(heading); //convert from polar to cartesian coordinates
    deltaY = deltaDistance * (float)sin(heading);
    X += deltaX; //add to running coordinate totals
    Y += deltaY;
    if(heading>PI) //keep heading between pi and -pi
        heading -= TWO_PI;
    else if (heading <= -PI)
        heading += TWO_PI;
    printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading); //print coorindates after update
}

int Navigate_Pololu::getX(){
    return (int)X;
}

int Navigate_Pololu::getY(){
    return (int)Y;
}

int Navigate_Pololu::getHeading(){
    return (int)((heading/PI)*180); //returns heading in degrees
}

void Navigate_Pololu::calibratePos(){
    //m3pi robot(D7, robotTx, robotRx); //create new m3pi object with D7 unused pin for reset
    wait(1);
    //align wheels
    while(leftOpto){
        robot.left_motor(0.2);
        wait(0.01);
    }
    robot.stop();
    wait(0.1);
    while(rightOpto){
        robot.right_motor(0.2);
        wait(0.01);
    }
    robot.stop();
}