version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCALIZE.cpp
- Revision:
- 15:7729da55873a
- Parent:
- 13:c62f975dfcfe
diff -r 4839989ae907 -r 7729da55873a LOCALIZE.cpp --- a/LOCALIZE.cpp Thu Mar 24 03:54:29 2016 +0000 +++ b/LOCALIZE.cpp Thu Mar 24 07:20:51 2016 +0000 @@ -1,22 +1,17 @@ #include "LOCALIZE.h" -DigitalIn sw1(p20); -DigitalIn sw2(p19); -DigitalIn sw3(p18); -DigitalIn sw4(p17); - DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); -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) +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) { - sw1.mode(PullUp); - sw2.mode(PullUp); - sw3.mode(PullUp); - sw4.mode(PullUp); + _sw1.mode(PullUp); + _sw2.mode(PullUp); + _sw3.mode(PullUp); + _sw4.mode(PullUp); } int LOCALIZE::init (void) @@ -69,8 +64,7 @@ _angle=-1; break; } - //_xya.a=_angle-FRAME_OFF; - //xya->a=_xya.a; + //xya->a=_angle; xya->a=_euler.h; _xya.a=xya->a; } @@ -84,30 +78,30 @@ 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; - if(!sw1 && !sw2) + if(!_sw1 && !_sw2) _rx=RX_OFF; - else if(!sw3 && !sw4) + 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; - if(!sw1 && !sw2) + if(!_sw1 && !_sw2) _ry=FRAME_H-RY_OFF; - else if(!sw3 && !sw4) + 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; - if(!sw1 && !sw2) + if(!_sw1 && !_sw2) _rx=FRAME_W-RX_OFF; - else if(!sw3 && !sw4) + 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; - if(!sw1 && !sw2) + if(!_sw1 && !_sw2) _ry=RY_OFF; - else if(!sw3 && !sw4) + else if(!_sw3 && !_sw4) _ry=FRAME_H-RY_OFF; } else { //error @@ -140,8 +134,8 @@ 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