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:
- 41:2ec22c53aa26
- Parent:
- 39:ae3f6bb56da0
- Child:
- 42:ebb25729b183
- Child:
- 44:9901ef8071c1
--- a/AigamozuControlPackets.cpp Wed May 25 14:33:23 2016 +0000 +++ b/AigamozuControlPackets.cpp Fri May 27 15:19:55 2016 +0000 @@ -217,10 +217,8 @@ const int straight = 8000, turning = 14000, wait = 9000; - if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){ - out_flag = false; - out_count_flag = false; - }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){ + if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman) + && AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){ out_flag = false; out_count_flag = false; }else{//if robot is out @@ -316,21 +314,20 @@ //Check Hit Point Area -bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){ +bool AigamozuControlPackets::checkGpsHit( vector2D A, vector2D B, vector2D C, vector2D 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 CP = AigamozuControlPackets::sub_vector(P, C); - vector2D CA = AigamozuControlPackets::sub_vector(A, C); - vector2D AP = AigamozuControlPackets::sub_vector(P, A); + // 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; + double c2 = BC.x * BP.y - BC.y * BP.x; - if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) { + if( ( c1 > 0 && c2 > 0) || ( c1 < 0 && c2 < 0) ) { return true; }