forward
Dependencies: VNH5019
Fork of AigamozuControlPackets by
Diff: AigamozuControlPackets.cpp
- Revision:
- 20:fec2d6dec897
- Parent:
- 19:13b24b50800e
- Child:
- 21:be08b5c100fe
diff -r 13b24b50800e -r fec2d6dec897 AigamozuControlPackets.cpp --- a/AigamozuControlPackets.cpp Wed May 13 04:08:12 2015 +0000 +++ b/AigamozuControlPackets.cpp Wed May 13 06:28:14 2015 +0000 @@ -54,19 +54,19 @@ double AigamozuControlPackets::get_agzCov_longi(){ return agzCov.y; } - + +double AigamozuControlPackets::get_basePoint_lati(int i){ + return basePoint[i].x; + } +double AigamozuControlPackets::get_basePoint_longi(int i){ + return basePoint[i].y; + } double AigamozuControlPackets::get_basePointKalman_lati(int i){ return basePointKalman[i].x; } double AigamozuControlPackets::get_basePointKalman_longi(int i){ return basePointKalman[i].y; } -double AigamozuControlPackets::get_basePoint_lati(int i){ - return basePoint[i].x; - } -double AigamozuControlPackets::get_basePoint_longi(int i){ - return basePoint[i].y; - } ////////////////////////////// // set関数 @@ -75,6 +75,17 @@ agzCov.x = cov_lati; agzCov.y = cov_longi; } + +void AigamozuControlPackets::set_agzAutoGPS(){ + agzPoint.x += 0.00001; + //agzPoint.y += 0.001; +} + +void AigamozuControlPackets::set_agzKalmanGPS(){ + agzPointKalman.x += 0.00001; + //agzPointKalman.y += 0.001; + +} ////////////////////////////// // Controller/Base -> Robot // ////////////////////////////// @@ -174,8 +185,7 @@ else randomCount = 0; } - -void AigamozuControlPackets::gpsAuto(){ +bool AigamozuControlPackets::gpsAuto(){ /* _agzSheild.changeSpeed(2,128,2,128):for moving robot @@ -185,50 +195,16 @@ bool out_flag = true; static bool out_count_flag = false; - int i; - /* - printf("Check Start\n"); - 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); - */ -printf("normal\n"); - printf("-1, %f, %f\n",agzPoint.x,agzPoint.y); - for(i = 0; i < 5; i++){ - printf(" %d, %f, %f\n", i, basePoint[i].x,basePoint[i].y); - } - if(AigamozuControlPackets::checkGpsHit(basePoint[0],basePoint[1],basePoint[2],agzPoint)){ - printf("InArea\n"); + if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){ out_flag = false; out_count_flag = false; - }else if(AigamozuControlPackets::checkGpsHit(basePoint[2],basePoint[3],basePoint[0],agzPoint)){ - printf("InArea\n"); + }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){ out_flag = false; out_count_flag = false; }else{//if robot is out - printf("OutArea\n"); out_flag = true; } - printf("Kalman\n"); - printf("-1, %f, %f\n",agzPointKalman.x,agzPointKalman.y); - for(i = 0; i < 5; i++){ - printf(" %d, %f, %f\n", i, basePointKalman[i].x,basePointKalman[i].y); - } - if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){ - printf("InArea\n"); - //out_flag = false; - //out_count_flag = false; - }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){ - 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){ @@ -248,11 +224,10 @@ //if robot is inner _agzSheild.changeSpeed(1,64,1,64);//straight } + + return out_flag; } - - - //Update Robot Point @@ -354,12 +329,12 @@ case 2: //nowMode = AUTO_MODE; //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0); - eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0); + //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0); nowMode = AUTO_GPS_MODE; break; case 3: - eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2); + //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2); nowMode = AUTO_GPS_MODE; break;