Diff: LOCALIZE.cpp

Revision:

2:634c1adf89b2

Parent:

1:8966a48ce8d5

Child:

3:c58eb4be51de

--- a/LOCALIZE.cpp	Tue Mar 01 19:26:32 2016 +0000
+++ b/LOCALIZE.cpp	Wed Mar 02 07:21:05 2016 +0000
@@ -15,8 +15,12 @@
     char data[]= {0,0,0};
     if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0)
         return ERROR_RX_P;
+    if(_x_imu_i2c.read(R_N_ADDR<<1, data, 3)!=0)
+        return ERROR_RX_N;
     if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0)
         return ERROR_RY_P;
+    if(_y_i2c.read(R_N_ADDR<<1, data, 3)!=0)
+        return ERROR_RY_N;
     return 0;
 }
 
@@ -59,18 +63,24 @@
 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
 {
     char data[]= {0,0,0};
-    int _rx_p, _ry_p, _rx, _ry;
+    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
     get_angle(xya);
     if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
-        _rx=_rx_p;
+        _rx=_rx_n;
         _ry=_ry_p;
     } else if(abs(_xya.a-90)<R_ERROR) {
         _rx=_ry_p;