Revision 20:3f68738c4019, committed 2019-10-11

Comitter:

Tom0108

Date:

Fri Oct 11 07:29:15 2019 +0000

Parent:

19:340fc4370258

Commit message:

Gyro reset measures

Changed in this revision

diff -r 340fc4370258 -r 3f68738c4019 main.cpp
--- a/main.cpp	Wed Oct 09 09:00:01 2019 +0000
+++ b/main.cpp	Fri Oct 11 07:29:15 2019 +0000
@@ -95,12 +95,14 @@
         rawyaw[1]=rawyaw[0];
         rawyaw[0]=bno.euler.yaw;
 
-        //180～-180をまたいだ時
-        if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360;
-        else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360;
+        if(rawyaw[0]!=0) {
+            //180～-180をまたいだ時
+            if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360;
+            else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360;
 
-        yaw-=rawyaw[0]-rawyaw[1];
-        yaw=fmod(yaw, 360.0);
+            yaw-=rawyaw[0]-rawyaw[1];
+            yaw=fmod(yaw, 360.0);
+        }
 
         //ゾーン対応
         if(zone==0) PALETTE(RED);
@@ -547,18 +549,18 @@
         /* --------------電磁弁系-------------------- */
 
         /*タオルを引っ張るやつ*/
-        if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
-        else i2c.Out_Set(Out_Data,4,0);
+        if(sbdbt.R2)i2c.Out_Set(Out_Data,3,1);
+        else i2c.Out_Set(Out_Data,3,0);
 
         //タオル展開
-        if(unfold_f) i2c.Out_Set(Out_Data,3,1);
-        else i2c.Out_Set(Out_Data,3,0);
+        if(unfold_f) i2c.Out_Set(Out_Data,5,1);
+        else i2c.Out_Set(Out_Data,5,0);
 
         //エアー解放
         if(release_f) {
             PALETTE(YELLOW);
-            i2c.Out_Set(Out_Data,5,1);
-        } else i2c.Out_Set(Out_Data,5,0);
+            i2c.Out_Set(Out_Data,4,1);
+        } else i2c.Out_Set(Out_Data,4,0);
 
         if(!rod_hit) PALETTE(CYAN);
 
@@ -652,9 +654,9 @@
 //        pc.printf("yaw:%.1f %.1f",yaw, bno.euler.yaw);
 //            pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]);
 //        pc.printf("LX:%d, LY:%d, RX:%d, RY:%d",sbdbt.LX, sbdbt.LY, sbdbt.RX, sbdbt.RY);
-        for(int i=0; i<9; i++)
-            pc.printf("%d ",(In_Data+i)->in_data);
-        pc.printf("\n\r");
+//        for(int i=0; i<9; i++)
+//            pc.printf("%d ",(In_Data+i)->in_data);
+//        pc.printf("\n\r");
     }     // while(1)
 }       // int main()
 

diff -r 340fc4370258 -r 3f68738c4019 main.h
--- a/main.h	Wed Oct 09 09:00:01 2019 +0000
+++ b/main.h	Fri Oct 11 07:29:15 2019 +0000
@@ -82,9 +82,9 @@
     {0,0,0},        // 腕の上下
     {0,1,0},        // 爪
     {0,2,0},        // タオルの先
-    {0,3,0},        // タオル展開
     {1,0,0},        // タオルの中央
     {1,1,0},        // 全体
+    {1,2,0},        // タオル展開
     {41,0,0},        // g
     {41,1,0},        // r
     {41,2,0},        // b;