File content as of revision 2:f34a22b28ac5:

#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),
    mainMicon(PC_10, PC_11, 115200),
    r1370(PC_6, PC_7),
    led(LED2)

{
    radian[0] = 0.0;
    radian[1] = -2.0*PI/3.0;
    radian[2] = 2.0*PI/3.0;
    ticker.attach(callback(this, &MeasuringWheel::computeXY),0.01);
//    ticker1.attach(callback(this, &MeasuringWheel::transmissionXY),0.1);
    mainMicon.attach(callback(this, &MeasuringWheel::resetpisithon));//受信割り込み
//    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()
{

    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;
//    r1370.update();
    yaw = r1370.getAngle();
    yaw *= PI/180;
    for(int i = 0; i < 3; i++) {
        subX = wheel[i]*cos(radian[i]);
        subY = wheel[i]*sin(radian[i]);
    }
    X += subX*cos((float)yaw) - subY*sin((float)yaw);
    Y += subX*sin((float)yaw) - subY*cos((float)yaw);
    X+=subX;
    Y+=subY;

    
    X_ = X + 32768;
    Y_ = Y + 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);

}

//void MeasuringWheel::rawAngle()
//{
//    r1370.update();
//    yaw = r1370.getRate();
//}

float MeasuringWheel::getjyroAngle()
{
    return yaw;
}

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::transmissionXY()
//{
//}

void MeasuringWheel::resetpisithon()
{
    get = mainMicon.getc();
    if(get == 'R') {
        X = 0;
        Y = 0;
//        Cdif = yaw + 540 - r1370.getRate();
//        Cdif %= 360;
//        Cdif -= 180;
        led = !led;
    }


}