Diff: AigamozuControlPackets.cpp

Revision:

7:200ce5c1f486

Parent:

6:f164a716be46

Child:

9:4f675487f06b

--- a/AigamozuControlPackets.cpp	Mon Jun 09 00:16:38 2014 +0000
+++ b/AigamozuControlPackets.cpp	Wed Jun 18 13:50:04 2014 +0000
@@ -7,6 +7,18 @@
 AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
     packetData = new uint8_t[50];
     packetLength = 0;
+    //--init base point--//
+    basePoint[0].x = 13956.2655;
+    basePoint[0].y = 3731.5060;
+    
+    basePoint[1].x = 13956.2898;
+    basePoint[1].y = 3731.5055;
+    
+    basePoint[2].x = 13956.2915;
+    basePoint[2].y = 3731.5245;
+    
+    basePoint[3].x = 13956.2680;
+    basePoint[3].y = 3731.5248;
     }
 
 
@@ -102,8 +114,60 @@
 
 void AigamozuControlPackets::gpsAuto(){
     
+    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");
+
+    
+    }
+ 
+ 
+ 
+ 
+    
+    
+//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);
+    
+    }
+
+
+//Check Hit Point Area
+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 )
+{
+    vector2D ret;
+    ret.x = a.x - b.x;
+    ret.y = a.y - b.y;
+    return ret;   
+}    
+
+
+
 
 //////////////////////////////
 //     Mode change          //
@@ -131,7 +195,7 @@
         break;
         
         case 3:
-            eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0);
+            eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,5.0);
             nowMode = AUTO_GPS_MODE;
         break;