File content as of revision 0:33c364521d16:

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

Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth):leftOpto(leftIn),rightOpto(rightIn){
    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);
}

uint8_t* Navigate_Pololu::getX(){
    //static_assert(sizeof(float) == 2);
    int f = 0; // whatever value
    uint8_t bytes[4];
    std::memcpy(bytes, &X,4);
    return bytes;
}

uint8_t* Navigate_Pololu::getY(){
    //static_assert(sizeof(float) == 2);
    float f = 0; // whatever value
    uint8_t bytes[4];
    std::memcpy(bytes, &Y,4);
    return bytes;
}

uint8_t* Navigate_Pololu::getHeading(){
    //static_assert(sizeof(float) == 2);
    float f = 0; // whatever value
    uint8_t bytes[4];
    std::memcpy(bytes, &heading,4);
    return bytes;
}