20150512 Nakazawa
Fork of AigamozuControlPackets by
Diff: AigamozuControlPackets.cpp
- Revision:
- 11:4d71c9cc3b4a
- Parent:
- 10:e77c664ee5e2
- Child:
- 12:eaab0ccb9255
--- 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: