File content as of revision 1:88d5becb1259:

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

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

PwmOut rot_prf(PB_3);             //motor_RF
PwmOut rot_nrf(PA_10);
PwmOut rot_prb(PA_9);            //motor_RB
PwmOut rot_nrb(PB_6);
PwmOut rot_plf(PB_5);             //motor_LF
PwmOut rot_nlf(PB_4);
PwmOut rot_plb(PB_10);            //motor_LB
PwmOut rot_nlb(PA_8);

Ticker ticker;



//(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 omega_rf[100],omega_rb[100],omega_lf[100],omega_lb[100];
double delta_d=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_omega_rf=0,b_omega_rf=0,a_omega_rb=0,b_omega_rb=0;
double a_omega_lf=0,b_omega_lf=0,a_omega_lb=0,b_omega_lb=0;

void Turn(double a)
{
    if(a>0) {                    //a>0で前進
        rot_prf=a;
        rot_nrf=0;
        rot_prb=a;
        rot_nrb=0;
        rot_plf=a;
        rot_nlf=0;
        rot_plb=a;
        rot_nlb=0;


    } else {                     //a<0で後退
        rot_prf=0;
        rot_nrf=-a;
        rot_prb=0;
        rot_nrb=-a;
        rot_plf=0;
        rot_nlf=-a;
        rot_plb=0;
        rot_nlb=-a;
    }
}

void calOmega()
{
    ec_rf.calOmega();
    ec_rb.calOmega();
    ec_lf.calOmega();
    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*delta_d*counter);
            wait(1);
            omega_rf[counter]=ec_rf.getOmega();
            omega_rb[counter]=ec_rb.getOmega();
            omega_lf[counter]=ec_lf.getOmega();
            omega_lb[counter]=ec_lb.getOmega();
            printf("(duty=%f)_OmegaRF[%d]=%f_OmegaRB[%d]=%f_OmegaLF[%d]=%f_OmegaLB[%d]=%f\r\n",
                   delta_d*counter,counter,omega_rf[counter],counter,omega_rb[counter],
                   counter,omega_lf[counter],counter,omega_lb[counter]);

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

    }
    counter=1;

///////////////////計算////////////////////////////////////
    double omega_sum_rf=0,omega_sum_rb=0,omega_sum_lf=0,omega_sum_lb=0;
    double x_omega_rf=0,x_omega_rb=0,x_omega_lf=0,x_omega_lb=0;
    const int k_s=cal_start,k_f=cal_fin;
    int i1=k_s,i2=k_s,i3=k_s,i4=k_s;

    for( ; i1<=k_f; i1++) {
        omega_sum_rf=omega_sum_rf+omega_rf[i1];
    }
    printf("OmegaSumRF=%f",omega_sum_rf);

    for( ; i2<=k_f; i2++) {
        omega_sum_rb=omega_sum_rb+omega_rb[i2];
    }
    printf("__OmegaSumRB=%f",omega_sum_rb);

    for( ; i3<=k_f; i3++) {
        omega_sum_lf=omega_sum_lf+omega_lf[i3];
    }
    printf("__OmegaSumLF=%f",omega_sum_lf);

    for( ; i4<=k_f; i4++) {
        omega_sum_lb=omega_sum_lb+omega_lb[i4];
    }
    printf("__OmegaSumLB=%f\r\n",omega_sum_lb);


    i1=k_s;
    i2=k_s;
    i3=k_s;
    i4=k_s;

    for( ; i1<=k_f; i1++) {
        x_omega_rf=x_omega_rf+delta_d*i1*omega_rf[i1];
    }
    printf("XOmegaRF=%f",x_omega_rf);

    for( ; i2<=k_f; i2++) {
        x_omega_rb=x_omega_rb+delta_d*i2*omega_rb[i2];
    }
    printf("__XOmegaRB=%f",x_omega_rb);

    for( ; i3<=k_f; i3++) {
        x_omega_lf=x_omega_lf+delta_d*i3*omega_lf[i3];
    }
    printf("__XOmegaLF=%f",x_omega_lf);

    for( ; i4<=k_f; i4++) {
        x_omega_lb=x_omega_lb+delta_d*i4*omega_lb[i4];
    }
    printf("__XOmegaLB=%f\r\n",x_omega_lb);


    double duty_sigma=0,duty2_sigma=0,deno;
    int num;
    num=k_f-k_s;
    num=num++;

    i1=k_s;
    i2=k_s;

    for( ; i1<k_f+1; i1++) {
        duty_sigma=duty_sigma+delta_d*i1;
    }
    printf("duty_sigma= %f",duty_sigma);
    for( ; i2<k_f+1; i2++) {
        duty2_sigma=duty2_sigma+delta_d*delta_d*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_omega_rf=(duty_sigma*omega_sum_rf-num*x_omega_rf)/deno;
    b_omega_rf=(duty_sigma*x_omega_rf-duty2_sigma*omega_sum_rf)/deno;

    a_omega_rb=(duty_sigma*omega_sum_rb-num*x_omega_rb)/deno;
    b_omega_rb=(duty_sigma*x_omega_rb-duty2_sigma*omega_sum_rb)/deno;

    a_omega_lf=(duty_sigma*omega_sum_lf-num*x_omega_lf)/deno;
    b_omega_lf=(duty_sigma*x_omega_lf-duty2_sigma*omega_sum_lf)/deno;

    a_omega_lb=(duty_sigma*omega_sum_lb-num*x_omega_lb)/deno;
    b_omega_lb=(duty_sigma*x_omega_lb-duty2_sigma*omega_sum_lb)/deno;

    printf("a_OmegaRF= %f  ___  b_OmegaRF= %f \r\n",a_omega_rf,b_omega_rf);
    printf("a_OmegaRB= %f  ___  b_OmegaRB= %f \r\n",a_omega_rb,b_omega_rb);
    printf("a_OmegaLF= %f  ___  b_OmegaLF= %f \r\n",a_omega_lf,b_omega_lf);
    printf("a_OmegaLB= %f  ___  b_OmegaLB= %f \r\n",a_omega_lb,b_omega_lb);


    printf("(a_RF , b_RF)=( %f, %f ) \r\n",1/a_omega_rf,-b_omega_rf/a_omega_rf);
    printf("(a_RB , b_RB)=( %f, %f ) \r\n",1/a_omega_rb,-b_omega_rb/a_omega_rb);
    printf("(a_LF , b_LF)=( %f, %f ) \r\n",1/a_omega_lf,-b_omega_lf/a_omega_lf);
    printf("(a_LB , b_LB)=( %f, %f ) \r\n",1/a_omega_lb,-b_omega_lb/a_omega_lb);
}



int main()
{
    rot_prf.period_us(50);
    rot_nrf.period_us(50);
    rot_prb.period_us(50);
    rot_nrb.period_us(50);
    rot_plf.period_us(50);
    rot_nlf.period_us(50);
    rot_plb.period_us(50);
    rot_nlb.period_us(50);

    ticker.attach(&calOmega,0.05);


    while(toggle1==0) {
        wait(1);
        printf("waiting...\r\n");
    }
    direct=1;
    measureMotor(18,1,18);
    a_forward_rf=1/a_omega_rf;
    b_forward_rf=-b_omega_rf/a_omega_rf;
    a_forward_lf=1/a_omega_lf;
    b_forward_lf=-b_omega_lf/a_omega_lf;
    a_forward_rb=1/a_omega_rb;
    b_forward_rb=-b_omega_rb/a_omega_rb;
    a_forward_lb=1/a_omega_lb;
    b_forward_lb=-b_omega_lb/a_omega_lb;
    direct=-1;

    measureMotor(18,1,18);
    a_back_rf=1/a_omega_rf;
    b_back_rf=-b_omega_rf/a_omega_rf;
    a_back_lf=1/a_omega_lf;
    b_back_lf=-b_omega_lf/a_omega_lf;
    a_back_rb=1/a_omega_rb;
    b_back_rb=-b_omega_rb/a_omega_rb;
    a_back_lb=1/a_omega_lb;
    b_back_lb=-b_omega_lb/a_omega_lb;

    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);

}