Diff: LOCALIZE.cpp

Revision:

4:d70375cfa533

Parent:

3:c58eb4be51de

Child:

5:c89308dc1827

--- a/LOCALIZE.cpp	Thu Mar 03 01:00:32 2016 +0000
+++ b/LOCALIZE.cpp	Fri Mar 04 03:02:36 2016 +0000
@@ -1,10 +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(x_imu_i2c, imu_reset)
-{
-
-}
+    _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset) {}
 
 int LOCALIZE::init (void)
 {
@@ -28,7 +25,6 @@
 {
     _imu.get_Euler_Angles(&_euler);
     _imu.get_gravity(&_gravity);
-    int _angle=-1;
     unsigned int quad=0;
     if(_gravity.y>=0)
         quad&=~1;
@@ -66,18 +62,19 @@
     get_raw_xy();
     get_angle(xya);
     if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
-        _rx=_rx_n;
-        _ry=_ry_p;
+        _rx=_rx_p<_rx_n ? _rx_p : FRAME_W-RX_OFF-_rx_n;
+        _ry=_ry_p<_ry_n ? _ry_p : FRAME_H-RY_OFF-_ry_n;
     } else if(abs(_xya.a-90)<R_ERROR) {
-        _rx=_ry_p;
-        _ry=FRAME_H-RX_OFF-_rx_p;
+        _rx=_ry_p<_ry_n ? _ry_p : FRAME_W-RY_OFF-_ry_n;
+        _ry=_rx_p<_rx_n ? FRAME_H-RX_OFF-_rx_p : _rx_n;
     } else if(abs(_xya.a-180)<R_ERROR) {
-        _rx=FRAME_W-RX_OFF-_rx_p;
-        _ry=FRAME_H-RY_OFF-_ry_p;
+        _rx=_rx_p<_rx_n ? FRAME_W-RX_OFF-_rx_p : _rx_n;
+        _ry=_ry_p<_ry_n ? FRAME_H-RY_OFF-_ry_p : _ry_n;
     } else if(abs(_xya.a-270)<R_ERROR) {
-        _rx=FRAME_W-RY_OFF-_ry_p;
-        _ry=_rx_p;
+        _rx=_ry_p<_ry_n ? FRAME_W-RY_OFF-_ry_p : _ry_n;
+        _ry=_rx_p<_rx_n ? _rx_p : FRAME_H-RX_OFF-_rx_n; 
     } else {
+        //error
         _rx=_rx_p;
         _ry=_ry_p;
     }
@@ -91,15 +88,15 @@
 {
     char data[]= {0,0,0};
     _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
-    wait(0.1);
+    wait(R_WAIT);
     _rx_p=data[1];
     _x_imu_i2c.read(R_N_ADDR<<1, data, 3);
-    wait(0.1);
+    wait(R_WAIT);
     _rx_n=data[1];
     _y_i2c.read(R_P_ADDR<<1, data, 3);
-    wait(0.1);
+    wait(R_WAIT);
     _ry_p=data[1];
     _y_i2c.read(R_N_ADDR<<1, data, 3);
-    wait(0.1);
+    wait(R_WAIT);
     _ry_n=data[1];
 }
\ No newline at end of file