Diff: AigamozuControlPackets.cpp

Revision:

11:4d71c9cc3b4a

Parent:

10:e77c664ee5e2

Child:

12:eaab0ccb9255

diff -r e77c664ee5e2 -r 4d71c9cc3b4a AigamozuControlPackets.cpp
--- a/AigamozuControlPackets.cpp	Sat Apr 11 12:25:25 2015 +0000
+++ b/AigamozuControlPackets.cpp	Mon Apr 13 10:09:30 2015 +0000
@@ -8,17 +8,25 @@
     packetData = new uint8_t[50];
     packetLength = 0;
     //--init base point--//
-    basePoint[0].x = 13956.2655;
-    basePoint[0].y = 3731.5060;
+    //basePoint[0].x = 13956.2655;
+    //basePoint[0].y = 3731.5060;
+    basePoint[0].x = 100.2655;
+    basePoint[0].y = 30.5060;
     
-    basePoint[1].x = 13956.2898;
-    basePoint[1].y = 3731.5055;
+    //basePoint[1].x = 13956.2898;
+    //basePoint[1].y = 3731.5055;
+    basePoint[1].x = 200.2898;
+    basePoint[1].y = 30.5055;
     
-    basePoint[2].x = 13956.2915;
-    basePoint[2].y = 3731.5245;
+    //basePoint[2].x = 13956.2915;
+    //basePoint[2].y = 3731.5245;
+    basePoint[2].x = 200.2915;
+    basePoint[2].y = 40.5245;
     
-    basePoint[3].x = 13956.2680;
-    basePoint[3].y = 3731.5248;
+    //basePoint[3].x = 13956.2680;
+    //basePoint[3].y = 3731.5248;
+    basePoint[3].x = 100.2680;
+    basePoint[3].y = 40.5248;
     }
 
 
@@ -124,12 +132,16 @@
     static bool out_count_flag = false;
     
     printf("Check Start\n");
-    printf("%f %f\n",agzPoint.x,agzPoint.y);
+    printf("agzPoint=%f,%f\n",agzPoint.x,agzPoint.y);
+    printf("bsdrPoint1=%f,%f\n",basePoint[0].x,basePoint[0].y);
+    printf("bsdrPoint2=%f,%f\n",basePoint[1].x,basePoint[1].y);
+    printf("bsdrPoint3=%f,%f\n",basePoint[2].x,basePoint[2].y);
+    printf("bsdrPoint4=%f,%f\n",basePoint[3].x,basePoint[3].y);
     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)){
+    }else if(AigamozuControlPackets::checkGpsHit(basePoint[2],basePoint[3],basePoint[0],agzPoint)){
         printf("InArea\n");
         out_flag = false;
         out_count_flag = false;
@@ -164,11 +176,17 @@
     
     
 //Update Robot Point    
-void AigamozuControlPackets::reNewPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
-    agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0);
-    agzPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0);
+void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+    agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0);
+    agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0);
     
     }
+    
+//Updata Base Point   
+void AigamozuControlPackets::reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+    basePoint[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
+    basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
+}   
 
 
 //Check Hit Point Area
@@ -226,8 +244,10 @@
         break;
         
         case 2:
-            nowMode = AUTO_MODE;
-            eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
+            //nowMode = AUTO_MODE;
+            //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
+            eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,1.0);
+            nowMode = AUTO_GPS_MODE;
         break;
         
         case 3: