forward
Dependencies: VNH5019
Fork of AigamozuControlPackets by
Diff: AigamozuControlPackets.cpp
- Revision:
- 10:e77c664ee5e2
- Parent:
- 9:4f675487f06b
- Child:
- 11:4d71c9cc3b4a
--- a/AigamozuControlPackets.cpp Fri Apr 10 12:58:42 2015 +0000 +++ b/AigamozuControlPackets.cpp Sat Apr 11 12:25:25 2015 +0000 @@ -114,14 +114,50 @@ void AigamozuControlPackets::gpsAuto(){ + /* + _agzSheild.changeSpeed(2,128,2,128):for moving robot + */ + + Timer Automove_Timer; + + bool out_flag = true; + static bool out_count_flag = false; + 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"); - - + 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)){ + 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){ + Automove_Timer.reset(); + out_count_flag = true; + } + if(out_flag){ + if(Automove_Timer.read_ms() < 1000){ + _agzSheild.changeSpeed(1,64,2,64);//turn + }else if(Automove_Timer.read_ms() < 5000){ + _agzSheild.changeSpeed(1,64,1,64);//straight + }else if(Automove_Timer.read_ms() >= 5000){ + out_count_flag=false; + } + }else{ + //if robot is inner + _agzSheild.changeSpeed(1,64,1,64);//straight + } + +} + @@ -195,7 +231,7 @@ break; case 3: - eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,5.0); + eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2); nowMode = AUTO_GPS_MODE; break;