Diff: AigamozuControlPackets.cpp

Revision:

20:fec2d6dec897

Parent:

19:13b24b50800e

Child:

21:be08b5c100fe

--- 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;