Diff: AigamozuControlPackets.cpp

Revision:

48:ee5a6906273e

Parent:

47:4da5b1b048aa

Child:

50:3511be172d81

diff -r 4da5b1b048aa -r ee5a6906273e AigamozuControlPackets.cpp
--- a/AigamozuControlPackets.cpp	Fri Jun 10 09:06:01 2016 +0000
+++ b/AigamozuControlPackets.cpp	Sat Jun 11 03:40:16 2016 +0000
@@ -1,6 +1,9 @@
 #include "AigamozuControlPackets.h"
 #include "VNH5019.h"
 
+const int turn_time[3] = {1, 2, 3};
+int tt = 0;
+
 //////////////////////////////
 //          Init            //
 //////////////////////////////
@@ -188,7 +191,8 @@
     else randomCount = 0;
     
     }
-    
+
+/*    
 void AigamozuControlPackets::test_Auto(int flag){
 
         if(flag == 0){
@@ -204,6 +208,7 @@
             _agzSheild.changeSpeed(0, 64, 0, 64); 
         }
 }
+*/
 
 bool AigamozuControlPackets::gpsAuto(){
     
@@ -214,7 +219,12 @@
     //Timer Automove_Timer;
    // bool out_flag = true;
    // static bool out_count_flag = false;
-    const int straight = 7000, turning = 10000, wait = 8000;
+    const int straight = 7000, wait = 8000;
+    int turning = wait + 1000*turn_time[tt]; //decide turning time
+    tt++;
+    if(tt == 3){
+        tt = 0;
+    }
     
 
     if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)
@@ -241,10 +251,10 @@
             }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);
+                 _agzSheild.changeSpeed(2,speed,2, speed);
             }else if(Automove_Timer.read_ms() > 5000){
                 Move_Timer.reset();
-                _agzSheild.changeSpeed(0,64,0,64);
+                _agzSheild.changeSpeed(0,speed,0,speed);
                 out_count_flag=false;
                 out_flag=false;
                 printf("reset timer\n");
@@ -253,16 +263,16 @@
         //if robot is inner
            // _agzSheild.changeSpeed(1,64,1,64);//straight
             if(Move_Timer.read_ms() < straight){
-                _agzSheild.changeSpeed(1, 64, 1, 64); //straight
+                _agzSheild.changeSpeed(1, speed, 1, speed); //straight
             }
             else if(Move_Timer.read_ms() < wait){
-                _agzSheild.changeSpeed(0, 64, 0, 64); 
+                _agzSheild.changeSpeed(0, speed, 0, speed); 
             }
             else if(Move_Timer.read_ms() < turning){
-                _agzSheild.changeSpeed(1, 64, 1, 16); //Turn Right
+                _agzSheild.changeSpeed(1, speed, 1, 16); //Turn Right
             }   
             else if(Move_Timer.read_ms() > turning){
-                _agzSheild.changeSpeed(0, 64, 0, 64); 
+                _agzSheild.changeSpeed(0, speed, 0, speed); 
                 wait_ms(500);
                 Move_Timer.reset();
             }