Diff: main.cpp

Revision:

20:3f68738c4019

Parent:

19:340fc4370258

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