Diff: main.cpp

Revision:

7:897b9ded86e2

Parent:

6:213041054f97

Child:

8:102c19510dce

--- a/main.cpp	Mon Jun 11 11:32:07 2018 +0000
+++ b/main.cpp	Tue Jun 12 04:17:48 2018 +0000
@@ -373,9 +373,11 @@
     float   gyData[3];
     float ax = 0;
     float ay = 0;
+    float buf_ax[10];
     int cnt_loop = 0;
-    float pre_ax = 0;
-    float thre_bump = 1.0;
+    int cnt_back = 0;
+    float thre_bump = 2.0;
+    bool enble_ChangeMode = false;
     
     while(sw1 != 0)
     {
@@ -404,23 +406,45 @@
         ax = acData[0];
         ay = acData[1];
         
-        // Store pre frame value
-        pre_ax = acData[0];       
+        if(!enble_ChangeMode)
+        {
+            if(cnt_loop > 20)
+            {
+                enble_ChangeMode = true;
+                }
+            }
         
+        // ax Buffer Store
+        for(int i = 9; i > 0; i--)
+        {
+            buf_ax[i] = buf_ax[i - 1];
+            }
+        buf_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)
         {
             case 1:                 // Up Straight
-                pc.printf("Mode 1");
+                pc.printf("Mode 1 \n");
             
-                if(ay < -0.2){
-                        left = 0.9;
+                if(ay < -0.5){
+                        left = 0.97;
                         right = 1.0;
+                        pc.printf("Lean Right");
                     }
-                    else if(ay > 0.2){
+                    else if(ay > 0.5){
                         left = 1.0;
-                        right = 0.9;
+                        right = 0.97;
+                        pc.printf("Lean Left");
                     }
                     else
                     {
@@ -429,31 +453,34 @@
                         }
                         
                     // Judge Bump
-                    if( - pre_ax < -thre_bump)
+                    if(enble_ChangeMode)
                     {
-                        stt_Mode = 2;
-                        pc.printf(" 1 -> 2 ");
-                        }
+                        if(ax - buf_ax[5] < -thre_bump)
+                        {
+                            stt_Mode = 2;
+                            pc.printf("Mode 1 -> 2 \n");
+                            }
+                    }
                 
                 break;
             case 2:             // Up Back
-                pc.printf("Mode 2");
-                if(cnt_loop < 10)
+                pc.printf("Mode 2 \n");
+                if(cnt_back < 30)
                 {
                     left = -1.0;
                     right = -1.0;
-                    cnt_loop++;
+                    cnt_back++;
                     }
                     else{
-                            cnt_loop = 0;
+                            cnt_back = 0;
                             stt_Mode = 3;
-                            pc.printf(" 2 -> 3");
+                            pc.printf("Mode 2 -> 3 \n");
                         }
                 
                 break;
             case 3:                 // Up Rotate
-                pc.printf("Mode 3");
-                if(ax < 9.8 && ay > 0.1 && ay < -0.1)
+                pc.printf("Mode 3 \n");
+                if(!(ax < -9.5 && ay < 0.5 && ay >= -0.3))
                 {
                     left = 1.0;
                     right = 0;                    
@@ -461,19 +488,20 @@
                     else
                     {
                         stt_Mode = 4;
-                        pc.printf(" 3 - > 4");
+                        pc.printf("Mode 3 - > 4 \n");
+                        
                         }
                 
                 break;
             
             case 4:                 // Down Straight
-                pc.printf("Mode 4");
-                if(ay < -0.2){
+                pc.printf("Mode 4 \n");
+                if(ay < -0.5){
                         left = 1.0;
-                        right = 0.9;
+                        right = 0.97;
                     }
-                    else if(ay > 0.2){
-                        left = 0.9;
+                    else if(ay > 0.5){
+                        left = 0.97;
                         right = 1.0;
                     }
                     else
@@ -483,30 +511,31 @@
                         }
                         
                     // Judge Bump
-                    if(ax - pre_ax < -thre_bump)
+                    if(ax - buf_ax[5] < -thre_bump)
                     {
                         stt_Mode = 5;
+                        pc.printf("Mode 4 -> 5 \n");
                         }
                 
                 break;
             case 5:           
-                pc.printf("Mode 5");
-                if(cnt_loop < 10)
+                pc.printf("Mode 5 \n");
+                if(cnt_back < 30)
                 {
                     left = -1.0;
                     right = -1.0;
-                    cnt_loop++;
+                    cnt_back++;
                     }
                     else{
-                            cnt_loop = 0;
+                            cnt_back = 0;
                             stt_Mode = 6;
                             pc.printf(" 5 -> 6 \n");
                         }
                 
                 break;
             case 6:                 //
-                pc.printf("Mode 6");
-                if(ax < -9.8 && ay > 0.1 && ay < -0.1)
+                pc.printf("Mode 6 \n");
+                if(!(ax > 9.5 && ay < 0.5 && ay >= 0))
                 {
                     left = 0;
                     right = 1.0;                    
@@ -523,6 +552,12 @@
         }
 #endif
 
+        cnt_loop++;
+
+        if(cnt_loop > 2000)
+        {
+            cnt_loop = 0;
+            }
     }
     ModeLed = 1;
     left = 0.0;