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