Diff: AigamozuControlPackets.cpp

Revision:

47:4da5b1b048aa

Parent:

46:246051b4f24d

Child:

48:ee5a6906273e

diff -r 246051b4f24d -r 4da5b1b048aa AigamozuControlPackets.cpp
--- a/AigamozuControlPackets.cpp	Mon Jun 06 09:52:28 2016 +0000
+++ b/AigamozuControlPackets.cpp	Fri Jun 10 09:06:01 2016 +0000
@@ -214,7 +214,7 @@
     //Timer Automove_Timer;
    // bool out_flag = true;
    // static bool out_count_flag = false;
-    const int straight = 8000, turning = 14000, wait = 9000;
+    const int straight = 7000, turning = 10000, wait = 8000;
     
 
     if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)
@@ -234,13 +234,15 @@
     if(nowMode == AUTO_GPS_MODE){
          
         if(out_flag == true){
-            if(Automove_Timer.read_ms() <= 5000){
-                _agzSheild.changeSpeed(1,32,1,124);//turn left
+         /*   if(Automove_Timer.read_ms() <= 5000){
+                _agzSheild.changeSpeed(1,16,1,64);//turn left
             }else if(Automove_Timer.read_ms() <= 6000){
-                _agzSheild.changeSpeed(0,124,0,124);
+                _agzSheild.changeSpeed(0,64,0,64);
             }else if(Automove_Timer.read_ms() <= 10000){
-                _agzSheild.changeSpeed(1,124,1,124);//straight
-            }else if(Automove_Timer.read_ms() > 10000){
+                _agzSheild.changeSpeed(1,64,1,64);//straight*/
+            if(Automove_Timer.read_ms() <5000){
+                 _agzSheild.changeSpeed(2,64,2,64);
+            }else if(Automove_Timer.read_ms() > 5000){
                 Move_Timer.reset();
                 _agzSheild.changeSpeed(0,64,0,64);
                 out_count_flag=false;
@@ -251,13 +253,13 @@
         //if robot is inner
            // _agzSheild.changeSpeed(1,64,1,64);//straight
             if(Move_Timer.read_ms() < straight){
-                _agzSheild.changeSpeed(1, 124, 1, 124); //straight
+                _agzSheild.changeSpeed(1, 64, 1, 64); //straight
             }
             else if(Move_Timer.read_ms() < wait){
                 _agzSheild.changeSpeed(0, 64, 0, 64); 
             }
             else if(Move_Timer.read_ms() < turning){
-                _agzSheild.changeSpeed(1, 124, 1, 32); //Turn Right
+                _agzSheild.changeSpeed(1, 64, 1, 16); //Turn Right
             }   
             else if(Move_Timer.read_ms() > turning){
                 _agzSheild.changeSpeed(0, 64, 0, 64);