Diff: AigamozuControlPackets.cpp

Revision:

13:a5bc425540a7

Parent:

12:eaab0ccb9255

Child:

14:e441331aa4a1

--- a/AigamozuControlPackets.cpp	Mon Apr 13 11:33:06 2015 +0000
+++ b/AigamozuControlPackets.cpp	Mon Apr 20 10:36:16 2015 +0000
@@ -74,6 +74,21 @@
     packetLength =  RECEIVE_STATUS_COMMNAD_LENGTH;
 }
 
+void AigamozuControlPackets::createReceiveStatusCommandwithKalman(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL)
+{
+    //useing union's split::: long hoge.a(4byte) == uint8_t hoge.b[4] 
+    TEST_T latH,latL,lonH,lonL;
+    
+    latH.a = latitudeH;
+    latL.a = latitudeL;
+    lonH.a = longitudeH;
+    lonL.a = longitudeL;
+    
+    uint8_t tmp[] = {'A','G','S','K','F',fromID,'T',toID,'S',nowStatus,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69};
+    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
+    packetLength =  RECEIVE_STATUS_COMMNAD_LENGTH;
+}
+
 
 //////////////////////////////
 //    Using createPacket    //
@@ -139,10 +154,12 @@
     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 < 4; 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");
         out_flag = false;
@@ -155,6 +172,23 @@
         printf("OutArea\n");
         out_flag = true;
     }
+    printf("Kalman\n");
+    printf("-1, %f, %f\n",agzPointKalman.x,agzPointKalman.y);
+    for(i = 0; i < 4; 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){
@@ -168,6 +202,7 @@
             _agzSheild.changeSpeed(1,64,1,64);//straight
         }else if(Automove_Timer.read_ms() >= 5000){
             out_count_flag=false;
+            out_flag=false;
         }
     }else{
     //if robot is inner
@@ -194,6 +229,19 @@
     basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
 }   
 
+//Update Robot Point    
+void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+    agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0);
+    agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0);
+    
+    }
+    
+//Updata Base Point   
+void AigamozuControlPackets::reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+    basePointKalman[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
+    basePointKalman[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
+}   
+
 
 //Check Hit Point Area
 bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){