File content as of revision 7:e74a1688680d:

#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),
    serial(measuringTX, measuringRX),
    r1370(R1370TX, R1370RX)
{
    serial.baud(115200);
    serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
    radian[0] = 0.0;
    radian[1] = -2.0*PI/3.0;
    radian[2] = 2.0*PI/3.0;
    thread.start(callback(this, &MeasuringWheel::threadloop));
//    ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1);
    //mainMicon.attach(callback(this, &MeasuringWheel::resetposition));//受信割り込み
//    angleTiker.attach(callback(this, &MeasuringWheel::rawAngle),0.1);
}


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

void MeasuringWheel::computeXY()
{
    subY = 0;
    subX = 0;
    wheel[0] =  diameter[0]*PI/400.0*w1.getPulses();
    wheel[1] =  diameter[1]*PI/400.0*w2.getPulses();
    wheel[2] =  diameter[2]*PI/400.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;
    yawdegree = r1370.getAngle();
    if((yawdegree - beforeYaw) > 350)yawMode--;
    else if((yawdegree - beforeYaw) < -350)yawMode++;
    beforeYaw = yawdegree;
    nowYaw = 360*yawMode + yawdegree;
    yaw = -1.0*nowYaw;
    yaw *= PI/180;
    for(int i = 0; i < 3; i++) {
        subY += wheel[i]*sin(radian[i]);
        subX += wheel[i]*cos(radian[i]);
    }
    X += subX*cos((float)yaw) - subY*sin((float)yaw);
    Y += subX*sin((float)yaw) + subY*cos((float)yaw);

    X_ = X + 32768;
    Y_ = Y + 32768;
    upBitX = (X_ >> 8) & 0xff;
    downBitX = X_ & 0xff;
    upBitY = (Y_ >> 8) & 0xff;
    downBitY = Y_ & 0xff;
    txdata[0] = upBitX;
    txdata[1] = downBitX;
    txdata[2] = upBitY;
    txdata[3] = downBitY;
    txdata[4] = r1370.upbit();
    txdata[5] = r1370.downbit();
    serial.sendData(txdata, BUFFER_SIZE);

}

void MeasuringWheel::threadloop()
{
    while(true) {
        computeXY();
    }
}

float MeasuringWheel::getjyroAngle()
{
    return yawdegree;
//    return nowYaw;
}

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

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

float MeasuringWheel::getWheel1()
{
    return w1.getPulses();
}

float MeasuringWheel::getWheel2()
{
    return w2.getPulses();
}

float MeasuringWheel::getWheel3()
{
    return w3.getPulses();
}



void MeasuringWheel::resetposition()
{
    if(get == 'R') {
        X = 0;
        Y = 0;
    }


}