Diff: AigamozuControlPackets.cpp

Revision:

17:a6fa8cc96d94

Parent:

14:e441331aa4a1

Child:

18:01882120e6cf

--- a/AigamozuControlPackets.cpp	Sat Apr 25 01:05:51 2015 +0000
+++ b/AigamozuControlPackets.cpp	Tue May 12 01:02:47 2015 +0000
@@ -27,10 +27,42 @@
     //basePoint[3].y = 3731.5248;
     basePoint[3].x = 100.2680;
     basePoint[3].y = 40.5248;
+    
+    agzPoint.x=0;agzPoint.y=0;
+    agzPointKalman.x=0;agzPointKalman.y=0;
+    agzCov.x=0;agzCov.y=0;
     }
 
+//////////////////////////////
+// get関数
+//////////////////////////////
+double AigamozuControlPackets::get_agzPoint_lati(){
+    return agzPoint.x;
+    }
+double AigamozuControlPackets::get_agzPoint_longi(){
+    return agzPoint.y;
+    }
+double AigamozuControlPackets::get_agzPointKalman_lati(){
+    return agzPointKalman.x;
+    }
+double AigamozuControlPackets::get_agzPointKalman_longi(){
+    return agzPointKalman.y;
+    }
+double AigamozuControlPackets::get_agzCov_lati(){
+    return agzCov.x;
+}
+double AigamozuControlPackets::get_agzCov_longi(){
+    return agzCov.y;
+}
 
 //////////////////////////////
+// set関数
+//////////////////////////////
+void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){
+    agzCov.x = cov_lati;
+    agzCov.y = cov_longi;    
+}
+//////////////////////////////
 // Controller/Base -> Robot //
 //////////////////////////////
 
@@ -59,35 +91,30 @@
 // Robot -> Controller/Base //
 //////////////////////////////
 
-void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL)
+void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi)
 {
-    //useing union's split::: long hoge.a(4byte) == uint8_t hoge.b[4] 
-    TEST_T latH,latL,lonH,lonL;
+    UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data;
+    UNION_double_char covarLati_data,covarLongi_data;
     
-    latH.a = latitudeH;
-    latL.a = latitudeL;
-    lonH.a = longitudeH;
-    lonL.a = longitudeL;
-    
-    uint8_t tmp[] = {'A','G','S','R','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};
+    latitude_data.double_value=latitude;
+    longitude_data.double_value=longitude;
+    longitude_data.double_value=latitudeKalman;
+    longitudeKalman_data.double_value=longitudeKalman;
+    covarLati_data.double_value=covarLati;
+    covarLongi_data.double_value=covarLongi;
+      
+    uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowMode,71,80,83,
+                    latitude_data.char_value[0],latitude_data.char_value[1],latitude_data.char_value[2],latitude_data.char_value[3],latitude_data.char_value[4],latitude_data.char_value[5],latitude_data.char_value[6],latitude_data.char_value[7],
+                    longitude_data.char_value[0],longitude_data.char_value[1],longitude_data.char_value[2],longitude_data.char_value[3],longitude_data.char_value[4],longitude_data.char_value[5],longitude_data.char_value[6],longitude_data.char_value[7],
+                    latitudeKalman_data.char_value[0],latitudeKalman_data.char_value[1],latitudeKalman_data.char_value[2],latitudeKalman_data.char_value[3],latitudeKalman_data.char_value[4],latitudeKalman_data.char_value[5],latitudeKalman_data.char_value[6],latitudeKalman_data.char_value[7],
+                    longitudeKalman_data.char_value[0],longitudeKalman_data.char_value[1],longitudeKalman_data.char_value[2],longitudeKalman_data.char_value[3],longitudeKalman_data.char_value[4],longitudeKalman_data.char_value[5],longitudeKalman_data.char_value[6],longitudeKalman_data.char_value[7],      
+                    covarLati_data.char_value[0],covarLati_data.char_value[1],covarLati_data.char_value[2],covarLati_data.char_value[3],covarLati_data.char_value[4],covarLati_data.char_value[5],covarLati_data.char_value[6],covarLati_data.char_value[7],
+                    covarLongi_data.char_value[0],covarLongi_data.char_value[1],covarLongi_data.char_value[2],covarLongi_data.char_value[3],covarLongi_data.char_value[4],covarLongi_data.char_value[5],covarLongi_data.char_value[6],covarLongi_data.char_value[7],
+                    65,71,69};
     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
     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;
-}
 
 
 //////////////////////////////
@@ -156,7 +183,7 @@
     */
 printf("normal\n");
     printf("-1, %f, %f\n",agzPoint.x,agzPoint.y);
-    for(i = 0; i < 4; i++){
+    for(i = 0; i < 5; i++){
         printf(" %d, %f, %f\n", i, basePoint[i].x,basePoint[i].y);
     }
     
@@ -174,7 +201,7 @@
     }
     printf("Kalman\n");
     printf("-1, %f, %f\n",agzPointKalman.x,agzPointKalman.y);
-    for(i = 0; i < 4; i++){
+    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)){
@@ -196,11 +223,11 @@
         out_count_flag = true;
     }
     if(out_flag){
-        if(Automove_Timer.read_ms() < 1000){
-            _agzSheild.changeSpeed(1,64,2,64);//turn
-        }else if(Automove_Timer.read_ms() < 5000){
-            _agzSheild.changeSpeed(1,64,1,64);//straight
-        }else if(Automove_Timer.read_ms() >= 5000){
+        if(Automove_Timer.read_ms() < 5000){
+            _agzSheild.changeSpeed(2,64,2,64);//back
+        }else if(Automove_Timer.read_ms() < 6000){
+            _agzSheild.changeSpeed(1,64,2,64);//straight
+        }else if(Automove_Timer.read_ms() >= 6000){
             out_count_flag=false;
             out_flag=false;
         }
@@ -213,13 +240,12 @@
  
  
  
- 
     
     
 //Update Robot Point    
 void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
-    agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0);
-    agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0);
+    agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
+    agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
     
     }
     
@@ -231,8 +257,8 @@
 
 //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);
+    agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
+    agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
     
     }