Diff: LOCALIZE.cpp

Revision:

19:2dd81b864e14

Parent:

15:7729da55873a

Child:

20:10a305298e27

--- a/LOCALIZE.cpp	Tue Mar 29 02:12:08 2016 +0000
+++ b/LOCALIZE.cpp	Sun Apr 03 05:48:37 2016 +0000
@@ -1,9 +1,9 @@
 #include "LOCALIZE.h"
 
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+DigitalOut _led1(LED1);
+DigitalOut _led2(LED2);
+DigitalOut _led3(LED3);
+DigitalOut _led4(LED4);
 
 LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset, PinName sw1, PinName sw2, PinName sw3, PinName sw4):
     _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset), _sw1(sw1), _sw2(sw2), _sw3(sw3), _sw4(sw4)
@@ -19,8 +19,12 @@
     if(_imu.chip_ready()==0)
         return ERROR_IMU;
     _imu.set_mounting_position(MT_P0);
-    //_imu.change_fusion_mode(MODE_IMU);
+#if defined (IMU_GROUND)
     _imu.change_fusion_mode(MODE_NDOF);
+#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;
@@ -64,8 +68,11 @@
             _angle=-1;
             break;
     }
-    //xya->a=_angle;
+#if defined (IMU_GROUND)
     xya->a=_euler.h;
+#else
+    xya->a=_angle;
+#endif
     _xya.a=xya->a;
 }
 
@@ -76,35 +83,35 @@
     get_angle(xya);
     ledSw();
     if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
-        _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;
+        _rx=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? _rx_p : FRAME_W-RX_OFF-_rx_n;
+        _ry=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? _ry_p : FRAME_H-RY_OFF-_ry_n;
         if(!_sw1 && !_sw2)
             _rx=RX_OFF;
         else if(!_sw3 && !_sw4)
             _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;
+        _rx=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? _ry_p : FRAME_W-RY_OFF-_ry_n;
+        _ry=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? FRAME_H-RX_OFF-_rx_p : _rx_n;
         if(!_sw1 && !_sw2)
             _ry=FRAME_H-RY_OFF;
         else if(!_sw3 && !_sw4)
             _ry=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;
+        _rx=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? FRAME_W-RX_OFF-_rx_p : _rx_n;
+        _ry=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? FRAME_H-RY_OFF-_ry_p : _ry_n;
         if(!_sw1 && !_sw2)
             _rx=FRAME_W-RX_OFF;
         else if(!_sw3 && !_sw4)
             _rx=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;
+        _rx=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? FRAME_W-RY_OFF-_ry_p : _ry_n;
+        _ry=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? _rx_p : FRAME_H-RX_OFF-_rx_n;
         if(!_sw1 && !_sw2)
             _ry=RY_OFF;
         else if(!_sw3 && !_sw4)
             _ry=FRAME_H-RY_OFF;
     } else {
-        //error
+        //error last value
         _rx=_xya.x;
         _ry=_xya.x;
     }
@@ -129,13 +136,12 @@
     _y_i2c.read(R_N_ADDR<<1, data, 3);
     wait(R_WAIT);
     _ry_p=data[1];
-    //printf("%d %d %d %d\n",_rx_p,_rx_n,_ry_p,_ry_n);
 }
 
 void LOCALIZE::ledSw(void)
 {
-    led1=!_sw1;
-    led2=!_sw2;
-    led3=!_sw3;
-    led4=!_sw4;
+    _led1=!_sw1;
+    _led2=!_sw2;
+    _led3=!_sw3;
+    _led4=!_sw4;
 }
\ No newline at end of file