File content as of revision 0:3ad208cbea5f:

#include "measuring_wheel.h"

MeasuringWheel::MeasuringWheel(PinName channel1_1, PinName channel1_2, PinName channel2_1, PinName channel2_2, PinName channel3_1, PinName channel3_2,PinName R1370TX,PinName R1370RX):
    w1(channel1_1, channel1_2, NC, PPR, QEI::X4_ENCODING),
    w2(channel2_1, channel2_2, NC, PPR, QEI::X4_ENCODING),
    w3(channel3_1, channel3_2, NC, PPR, QEI::X4_ENCODING),
    r1370(R1370TX, R1370RX)
{
    miniX = 0;
    miniY = 0;
    X = 0;
    Y = 0;
    loopCounter = 0;

    /*けいそくりんの設定*/
    //ホイールの向き
    radian[0] = -1 * PIII / 2.0;
    radian[1] = PIII / 6.0;
    radian[2] = 5.0 * PIII / 6.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/(PPR*4.0)*w1.getPulses();
    wheel(1) =  -diameter[1]*PIII/(PPR*4.0)*w2.getPulses();
    wheel(2) =  -diameter[2]*PIII/(PPR*4.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);

    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;
    minusY = (Y + miniY) * -1;
}

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

double MeasuringWheel::getjyroAngle()
{
    return r1370.getAngle();
    //return phi;
}

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

double MeasuringWheel::getOutY()
{
    return -1*minusY;
}

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

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

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



void MeasuringWheel::resetposition()
{
    X = 0;
    Y = 0;
}