aigamozu / AigamozuControlPackets_2016

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 aigamozu

Diff: AigamozuControlPackets.cpp

Revision:
41:2ec22c53aa26
Parent:
39:ae3f6bb56da0
Child:
42:ebb25729b183
Child:
44:9901ef8071c1
diff -r 33c58ac420d3 -r 2ec22c53aa26 AigamozuControlPackets.cpp
--- 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;
     }