Diff: LOCALIZE.cpp

Revision:

15:7729da55873a

Parent:

13:c62f975dfcfe

--- 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