Diff: LOCALIZE.cpp

Revision:

3:c58eb4be51de

Parent:

2:634c1adf89b2

Child:

4:d70375cfa533

--- a/LOCALIZE.cpp	Wed Mar 02 07:21:05 2016 +0000
+++ b/LOCALIZE.cpp	Thu Mar 03 01:00:32 2016 +0000
@@ -62,22 +62,8 @@
 
 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
 {
-    char data[]= {0,0,0};
-    int _rx_p, _ry_p, _rx_n, _ry_n, _rx, _ry;
-    _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
-    //longer wait for longer distance
-    wait(0.1);
-    _rx_p=data[1];
-    _x_imu_i2c.read(R_N_ADDR<<1, data, 3);
-    wait(0.1);
-    _rx_n=data[1];
-    _y_i2c.read(R_P_ADDR<<1, data, 3);
-    wait(0.1);
-    _ry_p=data[1];
-//    _y_i2c.read(R_N_ADDR<<1, data, 3);
-//    wait(0.1);
-//    _ry_n=data[1];
-    //check angle again
+    int _rx, _ry;
+    get_raw_xy();
     get_angle(xya);
     if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
         _rx=_rx_n;
@@ -101,3 +87,19 @@
     xya->y=_xya.y;
 }
 
+void LOCALIZE::get_raw_xy(void)
+{
+    char data[]= {0,0,0};
+    _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
+    wait(0.1);
+    _rx_p=data[1];
+    _x_imu_i2c.read(R_N_ADDR<<1, data, 3);
+    wait(0.1);
+    _rx_n=data[1];
+    _y_i2c.read(R_P_ADDR<<1, data, 3);
+    wait(0.1);
+    _ry_p=data[1];
+    _y_i2c.read(R_N_ADDR<<1, data, 3);
+    wait(0.1);
+    _ry_n=data[1];
+}
\ No newline at end of file