File content as of revision 8:8ea251946b2a:

#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, 100, QEI::X4_ENCODING),
    w2(channel2_1, channel2_2, NC, 100, QEI::X4_ENCODING),
    w3(channel3_1, channel3_2, NC, 100, QEI::X4_ENCODING),
    serial(mainTX, mainRX),
    r1370(R1370TX, R1370RX)
{
    serial.baud(115200);
    serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
    miniX = 0;
    miniY = 0;
    X = 0;
    Y = 0;
    phi = 0;
    loopCounter = 0;

    /*けいそくりんの設定*/
    //ホイールの向き
    radian[0] = 0.0;
    radian[1] = -2.0*PIII/3.0;
    radian[2] = 2.0*PIII/3.0;
    //直径
    diameter[0] = 49.0;
    diameter[1] = 49.0;
    diameter[2] = 49.0;
    //中心からの距離
    distance[0] = 124.2;
    distance[1] = 124.2;
    distance[2] = 124.2;

    coefficient(0) = cos(radian[0]);
    coefficient(1) = cos(radian[1]);
    coefficient(2) = cos(radian[2]);
    coefficient(3) = sin(radian[0]);
    coefficient(4) = sin(radian[1]);
    coefficient(5) = sin(radian[2]);
    coefficient(6) = distance[0];
    coefficient(7) = distance[1];
    coefficient(8) = distance[2];

    dec = coefficient.fullPivHouseholderQr();

    thread.start(callback(this, &MeasuringWheel::threadloop));
}

void MeasuringWheel::computeXY()
{
    subY = 0;
    subX = 0;
    wheel(0) =  -diameter[0]*PIII/400.0*w1.getPulses();
    wheel(1) =  -diameter[1]*PIII/400.0*w2.getPulses();
    wheel(2) =  -diameter[2]*PIII/400.0*w3.getPulses();
    w1.reset();
    w2.reset();
    w3.reset();
    yawdegree = r1370.getAngle();
    yaw = -1.0*yawdegree;
    yaw *= PIII/180.0;

    ans = dec.solve(wheel);

    subX = ans(0);
    subY = ans(1);
    dphi = ans(2);

    loopCounter++;
    if(loopCounter > 1000000){
        loopCounter = 0;
        X += (miniX + subX*cos((double)yaw) - subY*sin((double)yaw));
        Y += (miniY + subX*sin((double)yaw) + subY*cos((double)yaw));
        miniX = 0;
        miniY = 0;
    }else{
        miniX += subX*cos((double)yaw) - subY*sin((double)yaw);
        miniY += subX*sin((double)yaw) + subY*cos((double)yaw);
    }

    minusX = (X + miniX) * -1.0; // 2019AのはXとYがマイナスなので
    minusY = (Y + miniY) * -1.0;
    phi += dphi;

    X_ = minusX + 32768;
    Y_ = minusY + 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();
    }
}

double MeasuringWheel::getjyroAngle()
{
    return yawdegree;
    //return phi;
}

double MeasuringWheel::getOutX()
{
    return minusX;
}

double MeasuringWheel::getOutY()
{
    return minusY;
}

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

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

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



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