File content as of revision 1:b6c1de65e591:

#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){
    calibratePos(robotTx, robotRx); //call routine to calibrate wheels
    X = 0.0f;
    Y = 0.0f;
    heading = 0.0f;
    leftCounts = 0;
    rightCounts = 0;
    previousLeftCounts = 0;
    previousRightCounts = 0;
    distancePerCount = (PI * wheelDiameter) / (float)countsPerRevolution;
    radiansPerCount = PI * (wheelDiameter/trackWidth)/countsPerRevolution;
    leftOpto.rise(this, &Navigate_Pololu::leftWheelInterrupt);
    rightOpto.rise(this, &Navigate_Pololu::rightWheelInterrupt);
    printf("DistancePerCount: %f, RadiansPerCounts: %f\n",distancePerCount,radiansPerCount);
}

void Navigate_Pololu::leftWheelInterrupt(){
    leftCounts++;
    UpdatePosition();
    previousLeftCounts = leftCounts;
}

void Navigate_Pololu::rightWheelInterrupt(){
    rightCounts++;
    UpdatePosition();
    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;
    deltaX = deltaDistance * (float)cos(heading);
    deltaY = deltaDistance * (float)sin(heading);
    X += deltaX;
    Y += deltaY;
    if(heading>PI)
        heading -= TWO_PI;
    else if (heading <= -PI)
        heading += TWO_PI;
    printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading);
}

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

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

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

void Navigate_Pololu::calibratePos(PinName robotTx,PinName robotRx){
    m3pi robot(D7, robotTx, robotRx);
    wait(1);
    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();
}