File content as of revision 0:f8551143755d:

#include "mbed.h"
#include "EC.h"
#define RESOLUTION 500

DigitalIn button(USER_BUTTON);
DigitalIn toggle1(PA_15,PullUp);
DigitalIn toggle2(PB_7,PullUp);

PwmOut rotPRF(PB_3);             //motor_RF
PwmOut rotNRF(PA_10);
PwmOut rotPRB(PA_9);            //motor_RB
PwmOut rotNRB(PB_6);
PwmOut rotPLF(PB_5);             //motor_LF
PwmOut rotNLF(PB_4);
PwmOut rotPLB(PB_10);            //motor_LB
PwmOut rotNLB(PA_8);

Ticker tickerRF;
Ticker tickerRB;
Ticker tickerLF;
Ticker tickerLB;


//(A層,B層,分解能)                         //
//Ec1multi EC_RF(PC_6,PC_8,RESOLUTION);  //1逓倍用class
//or
Ec2multi EC_RF(PC_6,PC_8,RESOLUTION);  //2逓倍用class
//or
//Ec4multi EC_RF(PC_6,PC_8,RESOLUTION);  //4逓倍用class


//(A層,B層,分解能)                         //
//Ec1multi EC_RB(PC_5,PC_9,RESOLUTION);  //1逓倍用class
//or
Ec2multi EC_RB(PC_5,PC_9,RESOLUTION);  //2逓倍用class
//or
//Ec4multi EC_RB(PC_5,PC_9,RESOLUTION);  //4逓倍用class

//(A層,B層,分解能)                         //
//Ec1multi EC_LF(PC_11,PC_7,RESOLUTION);  //1逓倍用class
//or
Ec2multi EC_LF(PC_11,PC_7,RESOLUTION);  //2逓倍用class
//or
//Ec4multi EC_LF(PC_11,PC_7,RESOLUTION);  //4逓倍用class


//(A層,B層,分解能)                         //
//Ec1multi EC_LB(PC_10,PC_12,RESOLUTION);  //1逓倍用class
//or
Ec2multi EC_LB(PC_10,PC_12,RESOLUTION);  //2逓倍用class
//or
//Ec4multi EC_LB(PC_10,PC_12,RESOLUTION);  //4逓倍用class


int counter=1;
int direct=1;        //方向指定
double OmegaRF[100],OmegaRB[100],OmegaLF[100],OmegaLB[100];
double deltaD=0.05;
double a_forward_rf,b_forward_rf,a_back_rf,b_back_rf;
double a_forward_lf,b_forward_lf,a_back_lf,b_back_lf;
double a_forward_rb,b_forward_rb,a_back_rb,b_back_rb;
double a_forward_lb,b_forward_lb,a_back_lb,b_back_lb;
double a_OmegaRF=0,b_OmegaRF=0,a_OmegaRB=0,b_OmegaRB=0;
double a_OmegaLF=0,b_OmegaLF=0,a_OmegaLB=0,b_OmegaLB=0;

void Turn(double a)
{
    if(a>0) {                    //a>0で前進
        rotPRF=a;
        rotNRF=0;
        rotPRB=a;
        rotNRB=0;
        rotPLF=a;
        rotNLF=0;
        rotPLB=a;
        rotNLB=0;


    } else {                     //a<0で後退
        rotPRF=0;
        rotNRF=-a;
        rotPRB=0;
        rotNRB=-a;
        rotPLF=0;
        rotNLF=-a;
        rotPLB=0;
        rotNLB=-a;
    }
}

void CalOmegaRF()
{
    EC_RF.calOmega();
}

void CalOmegaRB()
{
    EC_RB.calOmega();
}

void CalOmegaLF()
{
    EC_LF.calOmega();
}

void CalOmegaLB()
{
    EC_LB.calOmega();
}
void measureMotor(int sokuteisuu,int cal_start,int cal_fin)
{
    int n=sokuteisuu;
    for(; counter<=2*n; counter++) {

        if(counter<=n) {
            Turn(direct*deltaD*counter);
            wait(1);
            OmegaRF[counter]=EC_RF.getOmega();
            OmegaRB[counter]=EC_RB.getOmega();
            OmegaLF[counter]=EC_LF.getOmega();
            OmegaLB[counter]=EC_LB.getOmega();
            printf("(duty=%f)_OmegaRF[%d]=%f_OmegaRB[%d]=%f_OmegaLF[%d]=%f_OmegaLB[%d]=%f\r\n",
                   deltaD*counter,counter,OmegaRF[counter],counter,OmegaRB[counter],
                   counter,OmegaLF[counter],counter,OmegaLB[counter]);

        } else {
            Turn(direct*deltaD*(2*n-counter));
            wait(0.5);
        }

    }
    counter=1;

///////////////////計算////////////////////////////////////
    double OmegaSumRF=0,OmegaSumRB=0,OmegaSumLF=0,OmegaSumLB=0;
    double XOmegaRF=0,XOmegaRB=0,XOmegaLF=0,XOmegaLB=0;
    const int Ks=cal_start,Kf=cal_fin;
    int i1=Ks,i2=Ks,i3=Ks,i4=Ks;

    for( ; i1<=Kf; i1++) {
        OmegaSumRF=OmegaSumRF+OmegaRF[i1];
    }
    printf("OmegaSumRF=%f",OmegaSumRF);

    for( ; i2<=Kf; i2++) {
        OmegaSumRB=OmegaSumRB+OmegaRB[i2];
    }
    printf("__OmegaSumRB=%f",OmegaSumRB);

    for( ; i3<=Kf; i3++) {
        OmegaSumLF=OmegaSumLF+OmegaLF[i3];
    }
    printf("__OmegaSumLF=%f",OmegaSumLF);

    for( ; i4<=Kf; i4++) {
        OmegaSumLB=OmegaSumLB+OmegaLB[i4];
    }
    printf("__OmegaSumLB=%f\r\n",OmegaSumLB);


    i1=Ks;
    i2=Ks;
    i3=Ks;
    i4=Ks;

    for( ; i1<=Kf; i1++) {
        XOmegaRF=XOmegaRF+deltaD*i1*OmegaRF[i1];
    }
    printf("XOmegaRF=%f",XOmegaRF);

    for( ; i2<=Kf; i2++) {
        XOmegaRB=XOmegaRB+deltaD*i2*OmegaRB[i2];
    }
    printf("__XOmegaRB=%f",XOmegaRB);

    for( ; i3<=Kf; i3++) {
        XOmegaLF=XOmegaLF+deltaD*i3*OmegaLF[i3];
    }
    printf("__XOmegaLF=%f",XOmegaLF);

    for( ; i4<=Kf; i4++) {
        XOmegaLB=XOmegaLB+deltaD*i4*OmegaLB[i4];
    }
    printf("__XOmegaLB=%f\r\n",XOmegaLB);


    double duty_sigma=0,duty2_sigma=0,Deno;
    int Num;
    Num=Kf-Ks;
    Num=Num++;

    i1=Ks;
    i2=Ks;

    for( ; i1<Kf+1; i1++) {
        duty_sigma=duty_sigma+deltaD*i1;
    }
    printf("duty_sigma= %f",duty_sigma);
    for( ; i2<Kf+1; i2++) {
        duty2_sigma=duty2_sigma+deltaD*deltaD*i2*i2;
    }
    Deno=duty_sigma*duty_sigma-Num*duty2_sigma;
    printf("duty^2_sigma= %f\r\n",duty2_sigma);
    printf("Deno= %f\r\n",Deno);


    a_OmegaRF=(duty_sigma*OmegaSumRF-Num*XOmegaRF)/Deno;
    b_OmegaRF=(duty_sigma*XOmegaRF-duty2_sigma*OmegaSumRF)/Deno;

    a_OmegaRB=(duty_sigma*OmegaSumRB-Num*XOmegaRB)/Deno;
    b_OmegaRB=(duty_sigma*XOmegaRB-duty2_sigma*OmegaSumRB)/Deno;

    a_OmegaLF=(duty_sigma*OmegaSumLF-Num*XOmegaLF)/Deno;
    b_OmegaLF=(duty_sigma*XOmegaLF-duty2_sigma*OmegaSumLF)/Deno;

    a_OmegaLB=(duty_sigma*OmegaSumLB-Num*XOmegaLB)/Deno;
    b_OmegaLB=(duty_sigma*XOmegaLB-duty2_sigma*OmegaSumLB)/Deno;

    printf("a_OmegaRF= %f  ___  b_OmegaRF= %f \r\n",a_OmegaRF,b_OmegaRF);
    printf("a_OmegaRB= %f  ___  b_OmegaRB= %f \r\n",a_OmegaRB,b_OmegaRB);
    printf("a_OmegaLF= %f  ___  b_OmegaLF= %f \r\n",a_OmegaLF,b_OmegaLF);
    printf("a_OmegaLB= %f  ___  b_OmegaLB= %f \r\n",a_OmegaLB,b_OmegaLB);


    printf("(a_RF , b_RF)=( %f, %f ) \r\n",1/a_OmegaRF,-b_OmegaRF/a_OmegaRF);
    printf("(a_RB , b_RB)=( %f, %f ) \r\n",1/a_OmegaRB,-b_OmegaRB/a_OmegaRB);
    printf("(a_LF , b_LF)=( %f, %f ) \r\n",1/a_OmegaLF,-b_OmegaLF/a_OmegaLF);
    printf("(a_LB , b_LB)=( %f, %f ) \r\n",1/a_OmegaLB,-b_OmegaLB/a_OmegaLB);
}



int main()
{
    rotPRF.period_us(50);
    rotNRF.period_us(50);
    rotPRB.period_us(50);
    rotNRB.period_us(50);
    rotPLF.period_us(50);
    rotNLF.period_us(50);
    rotPLB.period_us(50);
    rotNLB.period_us(50);

    tickerRF.attach(&CalOmegaRF,0.05);
    tickerRB.attach(&CalOmegaRB,0.05);
    tickerLF.attach(&CalOmegaLF,0.05);
    tickerLB.attach(&CalOmegaLB,0.05);





    while(toggle1==0) {
        wait(1);
        printf("waiting...\r\n");
    }
    direct=1;
    measureMotor(18,1,18);
    a_forward_rf=1/a_OmegaRF;
    b_forward_rf=-b_OmegaRF/a_OmegaRF;
    a_forward_lf=1/a_OmegaLF;
    b_forward_lf=-b_OmegaLF/a_OmegaLF;
    a_forward_rb=1/a_OmegaRB;
    b_forward_rb=-b_OmegaRB/a_OmegaRB;
    a_forward_lb=1/a_OmegaLB;
    b_forward_lb=-b_OmegaLB/a_OmegaLB;
    direct=-1;
    counter=1;
    measureMotor(18,1,18);
    a_back_rf=1/a_OmegaRF;
    b_back_rf=-b_OmegaRF/a_OmegaRF;
    a_back_lf=1/a_OmegaLF;
    b_back_lf=-b_OmegaLF/a_OmegaLF;
    a_back_rb=1/a_OmegaRB;
    b_back_rb=-b_OmegaRB/a_OmegaRB;
    a_back_lb=1/a_OmegaLB;
    b_back_lb=-b_OmegaLB/a_OmegaLB;

    printf("motor_rf\t(%f,%f,%f,%f);\r\n",a_forward_rf,b_forward_rf,a_back_rf,b_back_rf);
    printf("motor_rb\t(%f,%f,%f,%f);\r\n",a_forward_rb,b_forward_rb,a_back_rb,b_back_rb);
    printf("motor_lf\t(%f,%f,%f,%f);\r\n",a_forward_lf,b_forward_lf,a_back_lf,b_back_lf);
    printf("motor_lb\t(%f,%f,%f,%f);\r\n",a_forward_lb,b_forward_lb,a_back_lb,b_back_lb);

}