Diff: AigamozuControlPackets.cpp

Revision:

49:c69fcf98e6b7

Parent:

46:246051b4f24d

diff -r 246051b4f24d -r c69fcf98e6b7 AigamozuControlPackets.cpp
--- a/AigamozuControlPackets.cpp	Mon Jun 06 09:52:28 2016 +0000
+++ b/AigamozuControlPackets.cpp	Tue Aug 09 13:04:06 2016 +0000
@@ -54,6 +54,12 @@
 double AigamozuControlPackets::get_agzCov_longi(){
     return agzCov.y;
 }
+double AigamozuControlPackets::get_agzCov_lati_collect(){
+    return agzCov_collect.x;
+}
+double AigamozuControlPackets::get_agzCov_longi_collect(){
+    return agzCov_collect.y;
+}
    
 double AigamozuControlPackets::get_basePoint_lati(int i){
     return basePoint[i].x;
@@ -71,9 +77,21 @@
 //////////////////////////////
 // set関数
 //////////////////////////////
+void AigamozuControlPackets::set_agzCov_from_packet(uint8_t *cov_lati,uint8_t *cov_longi){
+    UNION_double_char lat_cov,longi_cov;
+    for(int i = 0;i < 8;i++){
+        lat_cov.char_value[i]=cov_lati[i];
+    }
+    for(int i = 0;i < 8;i++){
+        longi_cov.char_value[i]=cov_longi[i];
+    }
+    agzCov_collect.x = lat_cov.double_value;
+    agzCov_collect.y = longi_cov.double_value;
+}
+
 void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){
     agzCov.x = cov_lati;
-    agzCov.y = cov_longi;    
+    agzCov.y = cov_longi;
 }
 
 void AigamozuControlPackets::set_agzPointKalman_lati(double kalman_lati){
@@ -235,12 +253,12 @@
          
         if(out_flag == true){
             if(Automove_Timer.read_ms() <= 5000){
-                _agzSheild.changeSpeed(1,32,1,124);//turn left
-            }else if(Automove_Timer.read_ms() <= 6000){
+                _agzSheild.changeSpeed(2,124,2,124);//back
+      /*      }else if(Automove_Timer.read_ms() <= 6000){
                 _agzSheild.changeSpeed(0,124,0,124);
             }else if(Automove_Timer.read_ms() <= 10000){
-                _agzSheild.changeSpeed(1,124,1,124);//straight
-            }else if(Automove_Timer.read_ms() > 10000){
+                _agzSheild.changeSpeed(1,124,1,124);//straight*/
+            }else if(Automove_Timer.read_ms() > 5000){
                 Move_Timer.reset();
                 _agzSheild.changeSpeed(0,64,0,64);
                 out_count_flag=false;