Diff: main.cpp

Revision:

0:f8551143755d

Child:

1:88d5becb1259

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 03 07:11:43 2020 +0000
@@ -0,0 +1,286 @@
+#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);
+
+}
+
+