Diff: main.cpp

Revision:

12:252acb9c1201

Parent:

11:fcb6d8929c88

Child:

13:ce45cdb24996

--- a/main.cpp	Mon Jun 18 07:15:20 2018 +0000
+++ b/main.cpp	Mon Jun 18 08:43:33 2018 +0000
@@ -373,6 +373,8 @@
     float   gyData[3];
     float ax = 0;
     float ay = 0;
+    float az = 0;
+    float a = 0;
     float buf_ax[10];
     float mean_ax = 0;
     float sum_ax = 0;
@@ -380,7 +382,7 @@
     int cnt_loop = 0;
     int cnt_back = 0;
     int cnt_straight = 0;
-    float thre_bump = 2.0;
+    float thre_bump = 4.0;
     bool enable_ChangeMode = false;
     
         for(int i = 0; i < 10; i++)
@@ -416,12 +418,16 @@
         gyTimer.reset();
      
         //floatの値を合計5桁、小数点以下3桁の形でPCへ送信
-        pc.printf("%5.3f:%5.3f:%5.3f:%5.3f:%5.3f:%5.3f \n", acData[0], acData[1], acData[2],gyData[0],gyData[1],gyData[2]);  
+//        pc.printf("%5.3f:%5.3f:%5.3f:%5.3f:%5.3f:%5.3f \n", acData[0], acData[1], acData[2],gyData[0],gyData[1],gyData[2]);  
+//        pc.printf("%5.3f, ", acData[2]);
 
         ax = acData[0];
         ay = acData[1];
+        az = acData[2];
+//        a = sqrt(ax * ax + ay * ay + az * az);
+//        pc.printf("%5.3f, ", ax);
         
-        log_ax[cnt_loop] = ax;
+//        log_ax[cnt_loop] = ax;
         
 //        if(!enble_ChangeMode)
 //        {
@@ -440,7 +446,8 @@
         sum_ax += buf_ax[1];
         sum_ax -= buf_ax[9];
         mean_ax = sum_ax / 9;
-        
+        pc.printf("%5.3f, ", ax);
+//        
 //        for(int i = 29; i > 0; i--)
 //        {
 //            log_ax[i] = log_ax[i - 1];
@@ -449,10 +456,10 @@
         
         if(cnt_loop % 100 == 0)
         {
-                        for(int i = 0; i < 100; i++)
-            {
-                pc.printf("%5.3f, ", log_ax[i]);
-                }
+//                        for(int i = 0; i < 100; i++)
+//            {
+//                pc.printf("%5.3f, ", log_ax[i]);
+//                }
 //            pc.printf("loop : %d\n", log_ax);
 //            pc.printf("loop : %d\n", cnt_loop);
 //            pc.printf("loop : %d\n", cnt_loop);
@@ -493,7 +500,7 @@
                     // Judge Bump
                     if(enable_ChangeMode)
                     {                        
-                        if(ax - mean_ax > thre_bump)
+                        if(ax - buf_ax[5] < -thre_bump || ax - buf_ax[5] > thre_bump)
                         {
                             stt_Mode = 2;
                             pc.printf("Mode 1 -> 2 \n");
@@ -512,7 +519,7 @@
                 
                 break;
             case 2:             // Up Back
-                pc.printf("Mode 2 \n");
+//                pc.printf("Mode 2 \n");
                 if(cnt_back < 30)
                 {
                     left = 0;//-1.0;