Diff: LOCALIZE.cpp

Revision:

26:0ea6a550a99d

Parent:

25:f3bf8351bbd4

Child:

28:65daccc10f31

--- a/LOCALIZE.cpp	Tue Apr 05 02:30:40 2016 +0000
+++ b/LOCALIZE.cpp	Wed Apr 06 04:04:00 2016 +0000
@@ -1,5 +1,5 @@
 #include "LOCALIZE.h"
-
+ 
 LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset, PinName sw1, PinName sw2, PinName sw3, PinName sw4, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4):
     _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset), _sw1(sw1), _sw2(sw2), _sw3(sw3), _sw4(sw4), _led1(led1), _led2(led2), _led3(led3), _led4(led4)
 {
@@ -11,9 +11,10 @@
     wait(0.01);
     _led2=0;
 }
-
+ 
 int LOCALIZE::init (void)
 {
+    _imu.reset();
     if(_imu.chip_ready()==0)
         return ERROR_IMU;
     _imu.set_mounting_position(MT_P0);
@@ -22,7 +23,7 @@
 #else
     _imu.change_fusion_mode(MODE_IMU);
 #endif
-
+ 
     char data[]= {0,0,0};
     if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0)
         return ERROR_RX_P;
@@ -34,7 +35,7 @@
         return ERROR_RY_N;
     return 0;
 }
-
+ 
 void LOCALIZE::get_angle(LOCALIZE_xya *xya)
 {
     _imu.get_Euler_Angles(&_euler);
@@ -73,7 +74,7 @@
 #endif
     _xya.a=xya->a;
 }
-
+ 
 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
 {
     int _rx, _ry;
@@ -84,30 +85,30 @@
         _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;
         if(!_sw1 && !_sw2)
-            _rx=RX_OFF;
+            _rx=0;//RX_OFF;
         else if(!_sw3 && !_sw4)
-            _rx=FRAME_W-RX_OFF;
+            _rx=FRAME_W;//-RX_OFF;
     } else if(abs(_xya.a-270)<R_ERROR) {
         _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;
         if(!_sw1 && !_sw2)
-            _ry=FRAME_H-RY_OFF;
+            _ry=FRAME_H;//-RY_OFF;
         else if(!_sw3 && !_sw4)
-            _ry=RY_OFF;
+            _ry=0;//RY_OFF;
     } else if(abs(_xya.a-180)<R_ERROR) {
         _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;
         if(!_sw1 && !_sw2)
-            _rx=FRAME_W-RX_OFF;
+            _rx=FRAME_W;//-RX_OFF;
         else if(!_sw3 && !_sw4)
-            _rx=RX_OFF;
+            _rx=0;//RX_OFF;
     } else if(abs(_xya.a-90)<R_ERROR) {
         _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;
         if(!_sw1 && !_sw2)
-            _ry=RY_OFF;
+            _ry=0;//RY_OFF;
         else if(!_sw3 && !_sw4)
-            _ry=FRAME_H-RY_OFF;
+            _ry=FRAME_H;//-RY_OFF;
     } else {
         //error last value
         _rx=_xya.x;
@@ -130,7 +131,7 @@
     xya->x=_xya.x;
     xya->y=_xya.y;
 }
-
+ 
 void LOCALIZE::get_raw_xy(void)
 {
     char data[]= {0,0,0};
@@ -147,7 +148,7 @@
     wait(R_WAIT);
     _ry_p=data[1];
 }
-
+ 
 inline void LOCALIZE::ledSw(void)
 {
     _led1=!_sw1;