Revision 1:88d5becb1259, committed 2020-03-03

Comitter:

yuki0108

Date:

Tue Mar 03 08:33:34 2020 +0000

Parent:

0:f8551143755d

Commit message:

a

Changed in this revision

diff -r f8551143755d -r 88d5becb1259 main.cpp
--- a/main.cpp	Tue Mar 03 07:11:43 2020 +0000
+++ b/main.cpp	Tue Mar 03 08:33:34 2020 +0000
@@ -6,125 +6,112 @@
 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);
+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 tickerRF;
-Ticker tickerRB;
-Ticker tickerLF;
-Ticker tickerLB;
+Ticker ticker;
+
 
 
 //(A層,B層,分解能)                         //
-//Ec1multi EC_RF(PC_6,PC_8,RESOLUTION);  //1逓倍用class
+//Ec1multi ec_rf(PC_6,PC_8,RESOLUTION);  //1逓倍用class
 //or
-Ec2multi EC_RF(PC_6,PC_8,RESOLUTION);  //2逓倍用class
+Ec2multi ec_rf(PC_6,PC_8,RESOLUTION);  //2逓倍用class
 //or
-//Ec4multi EC_RF(PC_6,PC_8,RESOLUTION);  //4逓倍用class
+//Ec4multi ec_rf(PC_6,PC_8,RESOLUTION);  //4逓倍用class
 
 
 //(A層,B層,分解能)                         //
-//Ec1multi EC_RB(PC_5,PC_9,RESOLUTION);  //1逓倍用class
+//Ec1multi ec_rb(PC_5,PC_9,RESOLUTION);  //1逓倍用class
 //or
-Ec2multi EC_RB(PC_5,PC_9,RESOLUTION);  //2逓倍用class
+Ec2multi ec_rb(PC_5,PC_9,RESOLUTION);  //2逓倍用class
 //or
-//Ec4multi EC_RB(PC_5,PC_9,RESOLUTION);  //4逓倍用class
+//Ec4multi ec_rb(PC_5,PC_9,RESOLUTION);  //4逓倍用class
 
 //(A層,B層,分解能)                         //
-//Ec1multi EC_LF(PC_11,PC_7,RESOLUTION);  //1逓倍用class
+//Ec1multi ec_lf(PC_11,PC_7,RESOLUTION);  //1逓倍用class
 //or
-Ec2multi EC_LF(PC_11,PC_7,RESOLUTION);  //2逓倍用class
+Ec2multi ec_lf(PC_11,PC_7,RESOLUTION);  //2逓倍用class
 //or
-//Ec4multi EC_LF(PC_11,PC_7,RESOLUTION);  //4逓倍用class
+//Ec4multi ec_lf(PC_11,PC_7,RESOLUTION);  //4逓倍用class
 
 
 //(A層,B層,分解能)                         //
-//Ec1multi EC_LB(PC_10,PC_12,RESOLUTION);  //1逓倍用class
+//Ec1multi ec_lb(PC_10,PC_12,RESOLUTION);  //1逓倍用class
 //or
-Ec2multi EC_LB(PC_10,PC_12,RESOLUTION);  //2逓倍用class
+Ec2multi ec_lb(PC_10,PC_12,RESOLUTION);  //2逓倍用class
 //or
-//Ec4multi EC_LB(PC_10,PC_12,RESOLUTION);  //4逓倍用class
+//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 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_OmegaRF=0,b_OmegaRF=0,a_OmegaRB=0,b_OmegaRB=0;
-double a_OmegaLF=0,b_OmegaLF=0,a_OmegaLB=0,b_OmegaLB=0;
+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で前進
-        rotPRF=a;
-        rotNRF=0;
-        rotPRB=a;
-        rotNRB=0;
-        rotPLF=a;
-        rotNLF=0;
-        rotPLB=a;
-        rotNLB=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で後退
-        rotPRF=0;
-        rotNRF=-a;
-        rotPRB=0;
-        rotNRB=-a;
-        rotPLF=0;
-        rotNLF=-a;
-        rotPLB=0;
-        rotNLB=-a;
+        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 CalOmegaRF()
+void calOmega()
 {
-    EC_RF.calOmega();
-}
-
-void CalOmegaRB()
-{
-    EC_RB.calOmega();
+    ec_rf.calOmega();
+    ec_rb.calOmega();
+    ec_lf.calOmega();
+    ec_lb.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);
+            Turn(direct*delta_d*counter);
             wait(1);
-            OmegaRF[counter]=EC_RF.getOmega();
-            OmegaRB[counter]=EC_RB.getOmega();
-            OmegaLF[counter]=EC_LF.getOmega();
-            OmegaLB[counter]=EC_LB.getOmega();
+            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",
-                   deltaD*counter,counter,OmegaRF[counter],counter,OmegaRB[counter],
-                   counter,OmegaLF[counter],counter,OmegaLB[counter]);
+                   delta_d*counter,counter,omega_rf[counter],counter,omega_rb[counter],
+                   counter,omega_lf[counter],counter,omega_lb[counter]);
 
         } else {
-            Turn(direct*deltaD*(2*n-counter));
+            Turn(direct*delta_d*(2*n-counter));
             wait(0.5);
         }
 
@@ -132,122 +119,116 @@
     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;
+    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<=Kf; i1++) {
-        OmegaSumRF=OmegaSumRF+OmegaRF[i1];
+    for( ; i1<=k_f; i1++) {
+        omega_sum_rf=omega_sum_rf+omega_rf[i1];
     }
-    printf("OmegaSumRF=%f",OmegaSumRF);
+    printf("OmegaSumRF=%f",omega_sum_rf);
 
-    for( ; i2<=Kf; i2++) {
-        OmegaSumRB=OmegaSumRB+OmegaRB[i2];
+    for( ; i2<=k_f; i2++) {
+        omega_sum_rb=omega_sum_rb+omega_rb[i2];
     }
-    printf("__OmegaSumRB=%f",OmegaSumRB);
+    printf("__OmegaSumRB=%f",omega_sum_rb);
 
-    for( ; i3<=Kf; i3++) {
-        OmegaSumLF=OmegaSumLF+OmegaLF[i3];
+    for( ; i3<=k_f; i3++) {
+        omega_sum_lf=omega_sum_lf+omega_lf[i3];
     }
-    printf("__OmegaSumLF=%f",OmegaSumLF);
+    printf("__OmegaSumLF=%f",omega_sum_lf);
 
-    for( ; i4<=Kf; i4++) {
-        OmegaSumLB=OmegaSumLB+OmegaLB[i4];
+    for( ; i4<=k_f; i4++) {
+        omega_sum_lb=omega_sum_lb+omega_lb[i4];
     }
-    printf("__OmegaSumLB=%f\r\n",OmegaSumLB);
+    printf("__OmegaSumLB=%f\r\n",omega_sum_lb);
 
 
-    i1=Ks;
-    i2=Ks;
-    i3=Ks;
-    i4=Ks;
+    i1=k_s;
+    i2=k_s;
+    i3=k_s;
+    i4=k_s;
 
-    for( ; i1<=Kf; i1++) {
-        XOmegaRF=XOmegaRF+deltaD*i1*OmegaRF[i1];
+    for( ; i1<=k_f; i1++) {
+        x_omega_rf=x_omega_rf+delta_d*i1*omega_rf[i1];
     }
-    printf("XOmegaRF=%f",XOmegaRF);
+    printf("XOmegaRF=%f",x_omega_rf);
 
-    for( ; i2<=Kf; i2++) {
-        XOmegaRB=XOmegaRB+deltaD*i2*OmegaRB[i2];
+    for( ; i2<=k_f; i2++) {
+        x_omega_rb=x_omega_rb+delta_d*i2*omega_rb[i2];
     }
-    printf("__XOmegaRB=%f",XOmegaRB);
+    printf("__XOmegaRB=%f",x_omega_rb);
 
-    for( ; i3<=Kf; i3++) {
-        XOmegaLF=XOmegaLF+deltaD*i3*OmegaLF[i3];
+    for( ; i3<=k_f; i3++) {
+        x_omega_lf=x_omega_lf+delta_d*i3*omega_lf[i3];
     }
-    printf("__XOmegaLF=%f",XOmegaLF);
+    printf("__XOmegaLF=%f",x_omega_lf);
 
-    for( ; i4<=Kf; i4++) {
-        XOmegaLB=XOmegaLB+deltaD*i4*OmegaLB[i4];
+    for( ; i4<=k_f; i4++) {
+        x_omega_lb=x_omega_lb+delta_d*i4*omega_lb[i4];
     }
-    printf("__XOmegaLB=%f\r\n",XOmegaLB);
+    printf("__XOmegaLB=%f\r\n",x_omega_lb);
 
 
-    double duty_sigma=0,duty2_sigma=0,Deno;
-    int Num;
-    Num=Kf-Ks;
-    Num=Num++;
+    double duty_sigma=0,duty2_sigma=0,deno;
+    int num;
+    num=k_f-k_s;
+    num=num++;
 
-    i1=Ks;
-    i2=Ks;
+    i1=k_s;
+    i2=k_s;
 
-    for( ; i1<Kf+1; i1++) {
-        duty_sigma=duty_sigma+deltaD*i1;
+    for( ; i1<k_f+1; i1++) {
+        duty_sigma=duty_sigma+delta_d*i1;
     }
     printf("duty_sigma= %f",duty_sigma);
-    for( ; i2<Kf+1; i2++) {
-        duty2_sigma=duty2_sigma+deltaD*deltaD*i2*i2;
+    for( ; i2<k_f+1; i2++) {
+        duty2_sigma=duty2_sigma+delta_d*delta_d*i2*i2;
     }
-    Deno=duty_sigma*duty_sigma-Num*duty2_sigma;
+    deno=duty_sigma*duty_sigma-num*duty2_sigma;
     printf("duty^2_sigma= %f\r\n",duty2_sigma);
-    printf("Deno= %f\r\n",Deno);
+    printf("Deno= %f\r\n",deno);
 
 
-    a_OmegaRF=(duty_sigma*OmegaSumRF-Num*XOmegaRF)/Deno;
-    b_OmegaRF=(duty_sigma*XOmegaRF-duty2_sigma*OmegaSumRF)/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_OmegaRB=(duty_sigma*OmegaSumRB-Num*XOmegaRB)/Deno;
-    b_OmegaRB=(duty_sigma*XOmegaRB-duty2_sigma*OmegaSumRB)/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_OmegaLF=(duty_sigma*OmegaSumLF-Num*XOmegaLF)/Deno;
-    b_OmegaLF=(duty_sigma*XOmegaLF-duty2_sigma*OmegaSumLF)/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_OmegaLB=(duty_sigma*OmegaSumLB-Num*XOmegaLB)/Deno;
-    b_OmegaLB=(duty_sigma*XOmegaLB-duty2_sigma*OmegaSumLB)/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_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_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_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);
+    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()
 {
-    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);
+    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);
 
-    tickerRF.attach(&CalOmegaRF,0.05);
-    tickerRB.attach(&CalOmegaRB,0.05);
-    tickerLF.attach(&CalOmegaLF,0.05);
-    tickerLB.attach(&CalOmegaLB,0.05);
-
-
-
+    ticker.attach(&calOmega,0.05);
 
 
     while(toggle1==0) {
@@ -256,25 +237,25 @@
     }
     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;
+    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;
-    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;
+    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);