Diff: main.cpp

Revision:

9:965bfb378463

Parent:

8:102c19510dce

Child:

10:d443aea353a2

--- a/main.cpp	Tue Jun 12 04:24:05 2018 +0000
+++ b/main.cpp	Wed Jun 13 06:52:56 2018 +0000
@@ -374,10 +374,19 @@
     float ax = 0;
     float ay = 0;
     float buf_ax[10];
+    float mean_ax = 0;
+    float sum_ax = 0;
+//    float log_ax[30];
     int cnt_loop = 0;
     int cnt_back = 0;
+    int cnt_straight = 0;
     float thre_bump = 2.0;
-    bool enble_ChangeMode = false;
+    bool enable_ChangeMode = false;
+    
+        for(int i = 0; i < 10; i++)
+        {
+            buf_ax[i] = 0;
+        }
     
     while(sw1 != 0)
     {
@@ -401,18 +410,18 @@
         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]);  
 
         ax = acData[0];
         ay = acData[1];
         
-        if(!enble_ChangeMode)
-        {
-            if(cnt_loop > 20)
-            {
-                enble_ChangeMode = true;
-                }
-            }
+//        if(!enble_ChangeMode)
+//        {
+//            if(cnt_loop > 20)
+//            {
+//                enble_ChangeMode = true;
+//                }
+//            }
         
         // ax Buffer Store
         for(int i = 9; i > 0; i--)
@@ -420,15 +429,24 @@
             buf_ax[i] = buf_ax[i - 1];
             }
         buf_ax[0] = ax;
+        sum_ax += buf_ax[1];
+        sum_ax -= buf_ax[9];
+        mean_ax = sum_ax / 9;
         
-        if(cnt_loop % 100 == 0)
-        {
-            pc.printf("loop : %d\n", cnt_loop);
-            pc.printf("loop : %d\n", cnt_loop);
-            pc.printf("loop : %d\n", cnt_loop);
-            pc.printf("loop : %d\n", cnt_loop);
-            pc.printf("loop : %d\n", cnt_loop);
-            }
+//        for(int i = 29; i > 0; i--)
+//        {
+//            log_ax[i] = log_ax[i - 1];
+//            }
+//            log_ax[0] = ax;
+        
+//        if(cnt_loop % 100 == 0)
+//        {
+//            pc.printf("loop : %d\n", cnt_loop);
+//            pc.printf("loop : %d\n", cnt_loop);
+//            pc.printf("loop : %d\n", cnt_loop);
+//            pc.printf("loop : %d\n", cnt_loop);
+//            pc.printf("loop : %d\n", cnt_loop);
+//            }
         
 #if 1
         switch(stt_Mode)
@@ -436,15 +454,23 @@
             case 1:                 // Up Straight
                 pc.printf("Mode 1 \n");
             
-                if(ay < -0.5){
-                        left = 0.97;
+//            for(int i = 0; i < 30; i++)
+//            {
+//                pc.printf("%5.3f, ", log_ax[i]);
+//                }
+//                pc.printf("\n");
+//                pc.printf("\n");
+//                pc.printf("\n");
+            
+                if(ay > 0.5){
+                        left = 0.9;
                         right = 1.0;
-                        pc.printf("Lean Right");
+//                        pc.printf("Lean Right");
                     }
-                    else if(ay > 0.5){
+                    else if(ay < -0.5){
                         left = 1.0;
-                        right = 0.97;
-                        pc.printf("Lean Left");
+                        right = 0.9;
+//                        pc.printf("Lean Left");
                     }
                     else
                     {
@@ -453,23 +479,33 @@
                         }
                         
                     // Judge Bump
-                    if(enble_ChangeMode)
-                    {
-                        if(ax - buf_ax[5] < -thre_bump)
+                    if(enable_ChangeMode)
+                    {                        
+                        if(ax - mean_ax > thre_bump)
                         {
                             stt_Mode = 2;
                             pc.printf("Mode 1 -> 2 \n");
+                            
+                            enable_ChangeMode = false;
                             }
-                    }
+                    }else
+                    {
+                        cnt_straight++;
+                        if(cnt_straight > 100)
+                        {
+                            enable_ChangeMode = true;
+                            cnt_straight = 0;
+                            }
+                        }
                 
                 break;
             case 2:             // Up Back
                 pc.printf("Mode 2 \n");
                 if(cnt_back < 30)
                 {
-                    left = -1.0;
-                    right = -1.0;
-                    cnt_back++;
+                    left = 0;//-1.0;
+                    right = 0;//-1.0;
+                    //cnt_back++;
                     }
                     else{
                             cnt_back = 0;
@@ -480,7 +516,7 @@
                 break;
             case 3:                 // Up Rotate
                 pc.printf("Mode 3 \n");
-                if(!(ax < -9.5 && ay < 0.5 && ay >= -0.3))      // Change Using Gyro??
+                if(!(ax < -9.7/* && ay < 0.5 && ay >= -0.3*/))      // Change Using Gyro??
                 {
                     left = 1.0;
                     right = 0;                    
@@ -490,22 +526,18 @@
                         stt_Mode = 4;
                         pc.printf("Mode 3 - > 4 \n");
                         
-                        for(int i = 0; i < 10; i++)
-                        {
-                            buf_ax[i] = ax;
-                            }
                         }
                 
                 break;
             
             case 4:                 // Down Straight
                 pc.printf("Mode 4 \n");
-                if(ay < -0.5){
+                if(ay > 0.5){
                         left = 1.0;
-                        right = 0.97;
+                        right = 0.9;
                     }
-                    else if(ay > 0.5){
-                        left = 0.97;
+                    else if(ay < -0.5){
+                        left = 0.9;
                         right = 1.0;
                     }
                     else
@@ -515,10 +547,23 @@
                         }
                         
                     // Judge Bump
-                    if(ax - buf_ax[5] < -thre_bump)
+                    if(enable_ChangeMode)
                     {
-                        stt_Mode = 5;
-                        pc.printf("Mode 4 -> 5 \n");
+                        if(ax - buf_ax[1] > thre_bump /*&& (ax - buf_ax[2] > thre_bump)*/)
+                        {
+                            stt_Mode = 5;
+                            pc.printf("Mode 4 -> 5 \n");
+                            
+                            enable_ChangeMode = false;
+                            }
+                    }else
+                    {                        
+                        cnt_straight++;
+                        if(cnt_straight > 10)
+                        {
+                            enable_ChangeMode = true;
+                            cnt_straight = 0;
+                            }
                         }
                 
                 break;
@@ -539,7 +584,7 @@
                 break;
             case 6:                 //
                 pc.printf("Mode 6 \n");
-                if(!(ax > 9.5 && ay < 0.5 && ay >= 0))      // Change Using Gyro??
+                if(!(ax > 9.7 /*&& ay < 0.5 && ay >= 0*/))      // Change Using Gyro??
                 {
                     left = 0;
                     right = 1.0;                    
@@ -548,13 +593,13 @@
                     {
                         stt_Mode = 1;
                         pc.printf(" 6 -> 1 \n");
+//                        
+//                        for(int i = 0; i < 3; i++)
+//                        {
+//                            buf_ax[i] = ax;
+//                            }
+                        }
                         
-                        for(int i = 0; i < 10; i++)
-                        {
-                            buf_ax[i] = ax;
-                            }
-                        }
-                        }
                 
                 break;
             default: