Diff: main.cpp

Revision:

15:e98de98d9328

Parent:

14:dd4adc577e36

--- a/main.cpp	Fri Jun 22 05:56:32 2018 +0000
+++ b/main.cpp	Fri Jun 29 04:13:37 2018 +0000
@@ -466,13 +466,13 @@
 //                pc.printf("Mode 1 \n");
                         
                 if(ay > 0.5){
-                        left = 0.9;
+                        left = 0.8;
                         right = 1.0;
 //                        pc.printf("Lean Right");
                     }
                     else if(ay < -0.5){
                         left = 1.0;
-                        right = 0.9;
+                        right = 0.8;
 //                        pc.printf("Lean Left");
                     }
                     else
@@ -487,8 +487,8 @@
                         if(ay > 4/*ax - mean_ax < -thre_bump || ax - mean_ax > thre_bump*/)
                         {
                             stt_Mode = 2;
-                            pc.printf("Mode 1 -> 2 \n");
-                            cnt_back = 0;
+//                            pc.printf("Mode 1 -> 2 \n");
+//                            cnt_back = 0;
                             
                             enable_ChangeMode = false;
                             }
@@ -504,45 +504,44 @@
                 
                 break;
             case 2:             // Up Back
-                pc.printf("Mode 2 \n");
+//                pc.printf("Mode 2 \n");
 //                if(cnt_back < 120)
 //                {
                     left = -1.0;
                     right = -1.0;
 //                    cnt_back++;
-                    wait(0.5);
+                    wait(0.6);
 //                    }
 //                    else{
 //                            cnt_back = 0;
                             stt_Mode = 3;
-                            pc.printf("Mode 2 -> 3 \n");
+//                            pc.printf("Mode 2 -> 3 \n");
 //                        }
                 
                 break;
             case 3:                 // Up Rotate
-                pc.printf("Mode 3 \n");
+//                pc.printf("Mode 3 \n");
                 if(!(ax < -9.7 && ay < 0/* && ay >= -0.3*/))      // Change Using Gyro??
                 {
                     left = 1.0;
-                    right = 0;                    
+                    right = -1.0;                    
                     }
                     else
                     {
                         stt_Mode = 4;
-                        pc.printf("Mode 3 - > 4 \n");
+//                        pc.printf("Mode 3 - > 4 \n");
                         
                         }
                 
-                break;
-            
+                break;            
             case 4:                 // Down Straight
-                pc.printf("Mode 4 \n");
+//                pc.printf("Mode 4 \n");
                 if(ay > 0.5){
                         left = 1.0;
-                        right = 0.9;
+                        right = 0.8;
                     }
                     else if(ay < -0.5){
-                        left = 0.9;
+                        left = 0.8;
                         right = 1.0;
                     }
                     else
@@ -557,7 +556,7 @@
                         if(ay < -4/*ax - mean_ax < -thre_bump_back || ax - mean_ax > thre_bump_back*/)
                         {
                             stt_Mode = 5;
-                            pc.printf("Mode 4 -> 5 \n");
+//                            pc.printf("Mode 4 -> 5 \n");
                             
                             enable_ChangeMode = false;
                             }
@@ -573,26 +572,26 @@
                 
                 break;
             case 5:           
-                pc.printf("Mode 5 \n");
+//                pc.printf("Mode 5 \n");
 //                if(cnt_back < 200)
 //                {
                     left = -1.0;
                     right = -1.0;
-                    wait(0.9);
+                    wait(1.0);
 //                    cnt_back++;
 //                    }
 //                    else{
 //                            cnt_back = 0;
                             stt_Mode = 6;
-                            pc.printf(" 5 -> 6 \n");
+//                            pc.printf(" 5 -> 6 \n");
 //                        }
                 
                 break;
             case 6:                 //
-                pc.printf("Mode 6 \n");
+//                pc.printf("Mode 6 \n");
                 if(!(ax > 9.7 && ay < 0/* && ay >= 0*/))      // Change Using Gyro??
                 {
-                    left = 0;
+                    left = -1.0;
                     right = 1.0;                    
                     }
                     else