File content as of revision 5:f8c3aeb4e65f:

#include "OmniPosition.h"

OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
    RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD),
    readCounter(0),
    takeCounter(0),
    X(0),
    Y(0),
    theta(0.0)
{
    buffer = new char[OP_SERIAL_BUFFER_SIZE];
    data = new char[2];
    attach(callback(this, &OmniPosition::readData));
    assembleTicker.attach(callback(this, &OmniPosition::assemble), OP_RECEIVE_FREQ);
    sendTicker.attach(callback(this, &OmniPosition::send), OP_SEND_FREQ);
}

void OmniPosition::readData()
{
    buffer[(int)readCounter] = getc();
    readCounter = incrementCounter(readCounter);
}

void OmniPosition::assemble()
{
    //Find header
    headerCheck = false;
    headerPoint = 0xff;

    for(int i = 0; i < OP_SERIAL_BUFFER_SIZE; i++) {
        if(buffer[i] == OP_HEADER_FIRST_BYTE) {
            takeCounter = i;
            takeCounter = incrementCounter(takeCounter);
            if(buffer[(int)takeCounter] == OP_HEADER_SECOND_BYTE) {
                headerCheck = true;
                headerPoint = i;
            }
        }
    }
    if(headerPoint == 0xff) {
        return;
    }

    //assemble
    checksum = 0;
    takeCounter = headerPoint;  //firstheader
    takeCounter = incrementCounter(takeCounter);  //secondheader

    takeCounter = incrementCounter(takeCounter);  //secondheader
    data[0] = buffer[(int)takeCounter];
    takeCounter = incrementCounter(takeCounter);  //secondheader
    data[1] = buffer[(int)takeCounter];
    X = ((data[0]<<8)|data[1]) - 32768;
    checksum = (data[0] ^ data[1]);

    takeCounter = incrementCounter(takeCounter);  //secondheader
    data[0] = buffer[(int)takeCounter];
    takeCounter = incrementCounter(takeCounter);  //secondheader
    data[1] = buffer[(int)takeCounter];
    Y = ((data[0]<<8)|data[1]) - 32768;
    checksum = (checksum ^ data[0]);
    checksum = (checksum ^ data[1]);

    takeCounter = incrementCounter(takeCounter);  //secondheader
    data[0] = buffer[(int)takeCounter];
    takeCounter = incrementCounter(takeCounter);  //secondheader
    data[1] = buffer[(int)takeCounter];
    thetaint = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
    theta = thetaint / 100.0;
    if(theta > 180.0) {
        theta -= 655.0;
    }
    theta *= -M_PI / 180.0;
    if(theta > M_PI) theta -= 2 * M_PI;
    if(theta < -M_PI) theta += 2 * M_PI;
    if(theta > M_PI) theta -= 2 * M_PI;
    if(theta < -M_PI) theta += 2 * M_PI;
    checksum = (checksum ^ data[0]);
    checksum = (checksum ^ data[1]);

    takeCounter = incrementCounter(takeCounter);  //secondheader
    if(buffer[(int)takeCounter] != checksum) {
        X = bfrX;
        Y = bfrY;
        theta = bfrTheta;
    } else {
        bfrX = X;
        bfrY = Y;
        bfrTheta = theta;
    }
    
}

int OmniPosition::incrementCounter(int counter)
{
    ++counter;
    if(counter >= OP_SERIAL_BUFFER_SIZE) {
        counter -= OP_SERIAL_BUFFER_SIZE;
    }
    return counter;
}

void OmniPosition::send()
{
    if(resetSend) {
        putc('R');
        resetSend = false;
    } else {
        //putc(0);
    }
}

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

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

double OmniPosition::getTheta()
{
    return -theta;
}

void OmniPosition::reset()
{
    resetSend = true;
}