File content as of revision 0:505dd5510add:

#include "measuring_wheel.h"

MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2):
    w1(channel1_1, channel1_2, NC, 500, QEI::X4_ENCODING),
    w2(channel2_1, channel2_2, NC, 500, QEI::X4_ENCODING),
    w3(channel3_1, channel3_2, NC, 500, QEI::X4_ENCODING)

{
    radian[0] = 0;
    radian[1] = -2*PI/3;
    radian[2] = 2*PI/3;
    ticker.attach(callback(this, &MeasuringWheel::computeXY),0.001);
}


bool MeasuringWheel::wheelDiameter(float diameter1, float diameter2, float diameter3)
{
    diameter[0] = diameter1;
    diameter[1] = diameter2;
    diameter[2] = diameter3;
    return 1;
}

void MeasuringWheel::computeXY()
{

    wheel[0] =  diameter[0]*PI/2000.0*w1.getPulses();
    wheel[1] =  diameter[1]*PI/2000.0*w2.getPulses();
    wheel[2] =  diameter[2]*PI/2000.0*w3.getPulses();
    w1.reset();
    w2.reset();
    w3.reset();
    r = (wheel[0] + wheel[1] + wheel[2])/3.0;
    wheel[0] -= r;
    wheel[1] -= r;
    wheel[2] -= r;
    for(int i = 0; i < 3; i++) {
        X += wheel[i]*cos(radian[i]);
        Y += wheel[i]*sin(radian[i]);
    }
}

float MeasuringWheel::getOutX()
{
    return X;
}

float MeasuringWheel::getOutY()
{
    return Y;
}

TransmissionXY::TransmissionXY(PinName serialTX,PinName serialRX): mainMicon(serialTX, serialRX, 115200), led(LED2)
{
//    mainMicon.attach(callback(this, &TransmissionXY::resetpisithon));
}

void TransmissionXY::resetpisithon()
{
    get = mainMicon.getc();
    if(get == 'R') {
        ofsetX = X_;
        ofsetY = Y_;
        led = !led;
    }
    X_ -= ofsetX;
    Y_ -= ofsetY;

}

bool TransmissionXY::sendMeasuring(MeasuringWheel &request)
{

    X_ = request.getOutX()+32768;
    Y_ = request.getOutY()+32768;
    upBitX = (X_ >> 8) & 0xff;
    downBitX = X_ & 0xff;
    upBitY = (Y_ >> 8) & 0xff;
    downBitY = Y_ & 0xff;
    mainMicon.putc(72);
    mainMicon.putc(42);
    mainMicon.putc(upBitX);
    mainMicon.putc(downBitX);
    mainMicon.putc(upBitY);
    mainMicon.putc(downBitY);

    return 1;
}