version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCALIZE.cpp
- Revision:
- 13:c62f975dfcfe
- Parent:
- 10:cf44f4387bc7
- Child:
- 15:7729da55873a
--- a/LOCALIZE.cpp Wed Mar 23 16:25:48 2016 +0000 +++ b/LOCALIZE.cpp Wed Mar 23 18:12:47 2016 +0000 @@ -1,7 +1,23 @@ #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) {} + _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset) +{ + sw1.mode(PullUp); + sw2.mode(PullUp); + sw3.mode(PullUp); + sw4.mode(PullUp); +} int LOCALIZE::init (void) { @@ -64,22 +80,39 @@ int _rx, _ry; get_raw_xy(); 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; + 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; + 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; + 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; + if(!sw1 && !sw2) + _ry=RY_OFF; + else if(!sw3 && !sw4) + _ry=FRAME_H-RY_OFF; } else { //error - _rx=_rx_p; - _ry=_ry_p; + _rx=_xya.x; + _ry=_xya.x; } _xya.x=_rx; _xya.y=_ry; @@ -103,4 +136,12 @@ 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; } \ No newline at end of file