Diff: AigamozuControlPackets.cpp

Revision:

48:4695e72853c6

Parent:

47:36fdf8cd4336

Child:

49:3f097e4ad60f

--- a/AigamozuControlPackets.cpp	Mon Mar 13 17:55:25 2017 +0000
+++ b/AigamozuControlPackets.cpp	Fri Mar 17 09:02:41 2017 +0000
@@ -53,7 +53,7 @@
     ay_data.int_value=ay;
     az_data.int_value=az;
 
-    uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID, 
+    uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID, 
     gx_data.char_value[0],gx_data.char_value[1],
     gy_data.char_value[0],gy_data.char_value[1],
     gz_data.char_value[0],gz_data.char_value[1],
@@ -68,6 +68,31 @@
     }
      
 
+//////////////////////////////
+// Robot -> Xbee     
+// 受信したジャイロ・加速度データを変換する    //
+//////////////////////////////
+//Updata Base Point   
+void AigamozuControlPackets::GetData(uint8_t *gx,uint8_t *gy, uint8_t *gz,uint8_t *ax, uint8_t *ay,uint8_t *az){
+    UNION_int_char t_gx, t_gy, t_gz, t_ax, t_ay, t_az;
+    for(int i = 0;i < 2;i++){
+        t_gx.char_value[i]=gx[i];
+        t_gy.char_value[i]=gy[i];
+        t_gz.char_value[i]=gz[i];
+    }
+    for(int i = 0;i < 8;i++){
+        t_ax.char_value[i]=ax[i];
+        t_ay.char_value[i]=ay[i];
+        t_az.char_value[i]=az[i];
+    }
+    
+    r_gx = t_gx.int_value;
+    r_gy = t_gy.int_value;
+    r_gz = t_gz.int_value;
+    r_ax = t_ax.int_value;
+    r_ay = t_ay.int_value;
+    r_az = t_az.int_value;
+}  
 
 //////////////////////////////
 //   each mode interrupt    //