DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: LOCALIZE.cpp
- Revision:
- 19:2dd81b864e14
- Parent:
- 15:7729da55873a
- Child:
- 20:10a305298e27
diff -r f9012e93edb8 -r 2dd81b864e14 LOCALIZE.cpp --- 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