Diff: LOCALIZE.cpp

Revision:

1:8966a48ce8d5

Parent:

0:96d6eb224379

Child:

2:634c1adf89b2

--- a/LOCALIZE.cpp	Tue Mar 01 18:20:45 2016 +0000
+++ b/LOCALIZE.cpp	Tue Mar 01 19:26:32 2016 +0000
@@ -1,7 +1,7 @@
 #include "LOCALIZE.h"
 
 LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset):
-    _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu_reset(imu_reset), _imu(x_imu_i2c, imu_reset)
+    _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset)
 {
 
 }
@@ -12,40 +12,82 @@
         return ERROR_IMU;
     _imu.set_mounting_position(MT_P0);
     _imu.change_fusion_mode(MODE_IMU);
+    char data[]= {0,0,0};
+    if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0)
+        return ERROR_RX_P;
+    if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0)
+        return ERROR_RY_P;
     return 0;
 }
 
 void LOCALIZE::get_angle(LOCALIZE_xya *xya)
 {
-    _imu.get_Euler_Angles(&euler);
-    _imu.get_gravity(&gravity);
-    int angle=0;
+    _imu.get_Euler_Angles(&_euler);
+    _imu.get_gravity(&_gravity);
+    int _angle=-1;
     unsigned int quad=0;
-    if(gravity.y>=0)
+    if(_gravity.y>=0)
         quad&=~1;
     else
         quad|=1;
-    if(gravity.x>=0)
+    if(_gravity.x>=0)
         quad&=~(1<<1);
     else
         quad|=(1<<1);
     quad&=3;
     switch(quad) {
         case 0:
-            angle=euler.p;
+            _angle=_euler.p;
             break;
         case 1:
-            angle=180-euler.p;
+            _angle=180-_euler.p;
             break;
         case 2:
-            angle=360+euler.p;
+            _angle=360+_euler.p;
             break;
         case 3:
-            angle=180+abs(euler.p);
+            _angle=180+abs(_euler.p);
             break;
         default:
-            angle=-1;
+            _angle=-1;
             break;
     }
-    xya->a=angle;
+    _xya.a=_angle;
+    xya->a=_xya.a;
 }
+
+void LOCALIZE::get_xy(LOCALIZE_xya *xya)
+{
+    char data[]= {0,0,0};
+    int _rx_p, _ry_p, _rx, _ry;
+    _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
+    //longer wait for longer distance
+    wait(0.1);
+    _rx_p=data[1];
+    _y_i2c.read(R_P_ADDR<<1, data, 3);
+    wait(0.1);
+    _ry_p=data[1];
+    //check angle again
+    get_angle(xya);
+    if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
+        _rx=_rx_p;
+        _ry=_ry_p;
+    } else if(abs(_xya.a-90)<R_ERROR) {
+        _rx=_ry_p;
+        _ry=FRAME_H-RX_OFF-_rx_p;
+    } else if(abs(_xya.a-180)<R_ERROR) {
+        _rx=FRAME_W-RX_OFF-_rx_p;
+        _ry=FRAME_H-RY_OFF-_ry_p;
+    } else if(abs(_xya.a-270)<R_ERROR) {
+        _rx=FRAME_W-RY_OFF-_ry_p;
+        _ry=_rx_p;
+    } else {
+        _rx=_rx_p;
+        _ry=_ry_p;
+    }
+    _xya.x=_rx;
+    _xya.y=_ry;
+    xya->x=_xya.x;
+    xya->y=_xya.y;
+}
+