Diff: main.cpp

Revision:

5:fcc79e507610

Parent:

4:236e5c58e8ee

Child:

6:040d001acb12

--- a/main.cpp	Fri Aug 23 09:20:04 2019 +0000
+++ b/main.cpp	Sat Aug 24 00:30:20 2019 +0000
@@ -75,8 +75,10 @@
 //y_deg: 起動時から絶対の角度
 //yaw: 相対角 初期状態が0になるようにする
 //offset: 初期状態を0にするために引く値
-double y_deg, offset;
-int yaw=0;
+double y_deg[2], offset;
+double yaw=0;
+double sum=0, ave=0;
+int offset_cnt=0;
 
 //Turn_val: 補正の目標値
 //dif_val: 補正値との差
@@ -120,19 +122,33 @@
 int main()
 {
     //flipper1.attach(&flip1, 0.001);    // 割り込み
+
     while(1) {
         //自動系のタイマースタート
         tim.start();
         mpu.loop();
-        if(sbdbt.State_check()) {
 
-            for(int i=0; i<7; i++) i2c.In(In_Data,i);
+        y_deg[1] = y_deg[0];
+        y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI;
+        
+        //180～-180をまたいだ時
+        if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360;
+        else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360;
 
-//            y_deg = (double)mpu.ypr[0] * 180 / M_PI;
-//            yaw = offset-y_deg;
-            yaw=90*dir;
-            yaw%=360;
-            dif_val = y_deg-Turn_val;
+        if(sbdbt.State_check()) {
+            for(int i=0; i<7; i++) i2c.In(In_Data,i);
+            
+            //誤差をさんぷりんぐ
+            if(offset_cnt<1000) {
+                if(offset_cnt!=0) {
+                    sum+=(y_deg[0]-y_deg[1]);
+                    ave=sum/offset_cnt;
+                }
+                offset_cnt++;
+            } else yaw += (y_deg[0]-y_deg[1])-ave;
+//            yaw=90*dir;
+//            yaw%=360;
+            dif_val = y_deg[0]-Turn_val;
 
             lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180);
             ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180);
@@ -176,7 +192,7 @@
                 }     // if(g)
             }// if(LX !=64 || LY !=64)
             else if(sbdbt.RX != 64) {         // 旋回
-                Turn_val=y_deg;
+                Turn_val=y_deg[0];
                 mecanum_Turn();
             } else {
                 mecanum_Stop();
@@ -191,7 +207,7 @@
                     }
                 } else {
                     left_f=0;
-                    Turn_val=y_deg;
+                    Turn_val=y_deg[0];
                     mecanum_Stop();
                 }
             }
@@ -203,7 +219,7 @@
                     }
                 } else {
                     right_f=0;
-                    Turn_val=y_deg;
+                    Turn_val=y_deg[0];
                     mecanum_Stop();
                 }
             }
@@ -420,7 +436,7 @@
 //            pc.printf("pull %2d%2d ",pull,pull_f);
 //            pc.printf("grab %2d%2d ",grab,grab_f);
 //            pc.printf("kago %2d%2d ",kago,kago_f);
-//            pc.printf("deg:%f dif:%f ",y_deg,dif_val);
+            pc.printf("0:%f 1:%f ave:%f yaw:%f",y_deg[0],y_deg[1],ave,yaw);
 //            pc.printf("%2d ",dir);
             pc.printf("\n\r");
 
@@ -441,8 +457,8 @@
             /* ----------------ボタン系--------------- */
             //補正角リセット
             if(sbdbt.L1) {
-                Turn_val=y_deg;
-                offset=y_deg;
+                Turn_val=y_deg[0];
+                offset=y_deg[0];
             }
             //90度回転して補正の角度を固定する
             if(LEFTkey) {