2016_05_19ver Auto mode 10sec forward, 2sec stop, 2sec right turn Please change test_mode's right turn ppm
Dependencies: VNH5019 AigamozuControlPackets_2016
Dependents: Aigamozu_Robot_2016_ver1 GPSLOG_program AigamozuControlPackets_2016
Fork of AigamozuControlPackets by
Diff: AigamozuControlPackets.cpp
- Revision:
- 37:5654e31ad452
- Parent:
- 36:7d1e546620ed
- Child:
- 38:4e8cfe5e4bfb
--- a/AigamozuControlPackets.cpp Thu May 19 07:48:40 2016 +0000 +++ b/AigamozuControlPackets.cpp Mon May 23 08:57:20 2016 +0000 @@ -198,7 +198,7 @@ _agzSheild.changeSpeed(0, 64, 0, 64); } if(flag == 2){ - _agzSheild.changeSpeed(1, 16, 0, 16); //Turn Right + _agzSheild.changeSpeed(1, 64, 1, 16); //Turn Right } if(flag == 3){ _agzSheild.changeSpeed(0, 64, 0, 64); @@ -211,48 +211,68 @@ _agzSheild.changeSpeed(2,128,2,128):for moving robot */ - Timer Automove_Timer; + //Timer Automove_Timer; + // bool out_flag = true; + // static bool out_count_flag = false; + const int straight = 8000, turning = 14000, wait = 9000; - bool out_flag = true; - static bool out_count_flag = false; - + if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){ - out_flag = false; - out_count_flag = false; + out_flag = false; + out_count_flag = false; }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){ - out_flag = false; - out_count_flag = false; + out_flag = false; + out_count_flag = false; }else{//if robot is out - out_flag = true; + out_flag = true; } - + + //if robot is out: - if(out_flag == true || out_count_flag == false){ + if(out_flag == true && out_count_flag == false){ Automove_Timer.reset(); out_count_flag = true; } -/* if(nowMode == AUTO_GPS_MODE){ + if(nowMode == AUTO_GPS_MODE){ - if(out_flag){ - if(Automove_Timer.read_ms() < 5000){ - _agzSheild.changeSpeed(2,64,2,64);//back - }else if(Automove_Timer.read_ms() < 6000){ - _agzSheild.changeSpeed(1,64,1,64);//straight - }else if(Automove_Timer.read_ms() >= 6000){ - out_count_flag=false; - out_flag=false; - } - }else{ - //if robot is inner - _agzSheild.changeSpeed(1,64,1,64);//straight - } - } -*/ + if(out_flag == true){ + if(Automove_Timer.read_ms() <= 5000){ + _agzSheild.changeSpeed(1,16,1,64);//turn left + }else if(Automove_Timer.read_ms() <= 6000){ + _agzSheild.changeSpeed(0,64,0,64); + }else if(Automove_Timer.read_ms() <= 10000){ + _agzSheild.changeSpeed(1,64,1,64);//straight + }else if(Automove_Timer.read_ms() > 10000){ + Move_Timer.reset(); + _agzSheild.changeSpeed(0,64,0,64); + out_count_flag=false; + out_flag=false; + } + }else{ + //if robot is inner + // _agzSheild.changeSpeed(1,64,1,64);//straight + if(Move_Timer.read_ms() < straight){ + _agzSheild.changeSpeed(1, 64, 1, 64); //straight + } + else if(Move_Timer.read_ms() < wait){ + _agzSheild.changeSpeed(0, 64, 0, 64); + } + else if(Move_Timer.read_ms() < turning){ + _agzSheild.changeSpeed(1, 64, 1, 16); //Turn Right + } + else if(Move_Timer.read_ms() > turning){ + _agzSheild.changeSpeed(0, 64, 0, 64); + wait_ms(500); + Move_Timer.reset(); + } + } + } + return out_flag; } - - + + //Update Robot Point void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){ agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0); @@ -284,7 +304,7 @@ void AigamozuControlPackets::reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude){ UNION_double_char lat,longi; for(int i = 0;i < 8;i++){ - lat.char_value[i]=latitude[i]; + lat.char_value[i]=latitude[i]; } for(int i = 0;i < 8;i++){ longi.char_value[i]=longitude[i]; @@ -299,24 +319,24 @@ 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 ) {