Diff: AigamozuControlPackets.cpp

Revision:

37:5654e31ad452

Parent:

36:7d1e546620ed

Child:

38:4e8cfe5e4bfb

diff -r 7d1e546620ed -r 5654e31ad452 AigamozuControlPackets.cpp
--- a/AigamozuControlPackets.cpp	Thu May 19 07:48:40 2016 +0000
+++ b/AigamozuControlPackets.cpp	Mon May 23 08:57:20 2016 +0000
@@ -198,7 +198,7 @@
             _agzSheild.changeSpeed(0, 64, 0, 64); 
         }
         if(flag == 2){
-             _agzSheild.changeSpeed(1, 16, 0, 16); //Turn Right
+             _agzSheild.changeSpeed(1, 64, 1, 16); //Turn Right
         }   
         if(flag == 3){
             _agzSheild.changeSpeed(0, 64, 0, 64); 
@@ -211,48 +211,68 @@
     _agzSheild.changeSpeed(2,128,2,128):for moving robot
     */
     
-    Timer Automove_Timer;
+    //Timer Automove_Timer;
+   // bool out_flag = true;
+   // static bool out_count_flag = false;
+    const int straight = 8000, turning = 14000, wait = 9000;
     
-    bool out_flag = true;
-    static bool out_count_flag = false;
-    
+
     if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){
-        out_flag = false;
-        out_count_flag = false;
+            out_flag = false;
+            out_count_flag = false;
     }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){
-        out_flag = false;
-        out_count_flag = false;
+            out_flag = false;
+            out_count_flag = false;
     }else{//if robot is out 
-        out_flag = true;
+            out_flag = true;
     }
- 
+
+
     //if robot is out:
-    if(out_flag == true || out_count_flag == false){
+    if(out_flag == true && out_count_flag == false){
         Automove_Timer.reset();
         out_count_flag = true;
     }
-/*    if(nowMode == AUTO_GPS_MODE){
+    if(nowMode == AUTO_GPS_MODE){
          
-   if(out_flag){
-        if(Automove_Timer.read_ms() < 5000){
-            _agzSheild.changeSpeed(2,64,2,64);//back
-        }else if(Automove_Timer.read_ms() < 6000){
-            _agzSheild.changeSpeed(1,64,1,64);//straight
-        }else if(Automove_Timer.read_ms() >= 6000){
-            out_count_flag=false;
-            out_flag=false;
-        }
-    }else{
-    //if robot is inner
-        _agzSheild.changeSpeed(1,64,1,64);//straight
-    }
-    }   
-*/ 
+        if(out_flag == true){
+            if(Automove_Timer.read_ms() <= 5000){
+                _agzSheild.changeSpeed(1,16,1,64);//turn left
+            }else if(Automove_Timer.read_ms() <= 6000){
+                _agzSheild.changeSpeed(0,64,0,64);
+            }else if(Automove_Timer.read_ms() <= 10000){
+                _agzSheild.changeSpeed(1,64,1,64);//straight
+            }else if(Automove_Timer.read_ms() > 10000){
+                Move_Timer.reset();
+                _agzSheild.changeSpeed(0,64,0,64);
+                out_count_flag=false;
+                out_flag=false;
+            }
+        }else{
+        //if robot is inner
+           // _agzSheild.changeSpeed(1,64,1,64);//straight
+            if(Move_Timer.read_ms() < 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, 64, 1, 16); //Turn Right
+            }   
+            else if(Move_Timer.read_ms() > turning){
+                _agzSheild.changeSpeed(0, 64, 0, 64); 
+                wait_ms(500);
+                Move_Timer.reset();
+            }
+        }  
+    } 
+
     return out_flag;
  
 }
-    
-    
+        
+        
 //Update Robot Point    
 void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
     agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
@@ -284,7 +304,7 @@
 void AigamozuControlPackets::reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude){
     UNION_double_char lat,longi;
     for(int i = 0;i < 8;i++){
-        lat.char_value[i]=latitude[i];
+    lat.char_value[i]=latitude[i];
     }
     for(int i = 0;i < 8;i++){
         longi.char_value[i]=longitude[i];
@@ -299,24 +319,24 @@
 bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){
     vector2D AB = AigamozuControlPackets::sub_vector(B, A);
     vector2D BP = AigamozuControlPackets::sub_vector(P, B);
-
+    
     vector2D BC = AigamozuControlPackets::sub_vector(C, B);
     vector2D CP = AigamozuControlPackets::sub_vector(P, C);
-
+    
     vector2D CA = AigamozuControlPackets::sub_vector(A, C);
     vector2D AP = AigamozuControlPackets::sub_vector(P, A);
     
     double c1 = AB.x * BP.y - AB.y * BP.x;
     double c2 = BC.x * CP.y - BC.y * CP.x;
     double c3 = CA.x * AP.y - CA.y * AP.x;
-
+    
     if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) {    
         return true;
     }
     
     return false;
 
-    }
+}
 
 vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
 {