Diff: AigamozuControlPackets.cpp

Revision:

10:e77c664ee5e2

Parent:

9:4f675487f06b

Child:

11:4d71c9cc3b4a

--- a/AigamozuControlPackets.cpp	Fri Apr 10 12:58:42 2015 +0000
+++ b/AigamozuControlPackets.cpp	Sat Apr 11 12:25:25 2015 +0000
@@ -114,14 +114,50 @@
 
 void AigamozuControlPackets::gpsAuto(){
     
+   /*
+    _agzSheild.changeSpeed(2,128,2,128):for moving robot
+    */
+    
+    Timer Automove_Timer;
+    
+    bool out_flag = true;
+    static bool out_count_flag = false;
+    
     printf("Check Start\n");
     printf("%f %f\n",agzPoint.x,agzPoint.y);
-    if(AigamozuControlPackets::checkGpsHit(basePoint[0],basePoint[1],basePoint[2],agzPoint)) printf("InArea1\n");
-    if(AigamozuControlPackets::checkGpsHit(basePoint[0],basePoint[2],basePoint[3],agzPoint)) printf("InArea2\n");
-
-    
+    if(AigamozuControlPackets::checkGpsHit(basePoint[0],basePoint[1],basePoint[2],agzPoint)){
+        printf("InArea\n");
+        out_flag = false;
+        out_count_flag = false;
+    }else if(AigamozuControlPackets::checkGpsHit(basePoint[0],basePoint[2],basePoint[3],agzPoint)){
+        printf("InArea\n");
+        out_flag = false;
+        out_count_flag = false;
+    }else{//if robot is out 
+        printf("OutArea\n");
+        out_flag = true;
     }
  
+    //if robot is out:
+    if(out_flag == true || out_count_flag == false){
+        Automove_Timer.reset();
+        out_count_flag = true;
+    }
+    if(out_flag){
+        if(Automove_Timer.read_ms() < 1000){
+            _agzSheild.changeSpeed(1,64,2,64);//turn
+        }else if(Automove_Timer.read_ms() < 5000){
+            _agzSheild.changeSpeed(1,64,1,64);//straight
+        }else if(Automove_Timer.read_ms() >= 5000){
+            out_count_flag=false;
+        }
+    }else{
+    //if robot is inner
+        _agzSheild.changeSpeed(1,64,1,64);//straight
+    }
+ 
+}
+ 
  
  
  
@@ -195,7 +231,7 @@
         break;
         
         case 3:
-            eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,5.0);
+            eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2);
             nowMode = AUTO_GPS_MODE;
         break;