Antoine Laurens / Mbed 2 deprecated STRAIGHT_DRIVE_NO_SEP

drive down

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

Diff: LOCALIZE.cpp

Revision:
6:0602a9e8118b
Parent:
5:c89308dc1827
Child:
8:b36be08c44f8
--- a/LOCALIZE.cpp	Sun Mar 06 18:31:59 2016 +0000
+++ b/LOCALIZE.cpp	Wed Mar 16 22:05:23 2016 +0000
@@ -8,7 +8,8 @@
     if(_imu.chip_ready()==0)
         return ERROR_IMU;
     _imu.set_mounting_position(MT_P0);
-    _imu.change_fusion_mode(MODE_IMU);
+    //_imu.change_fusion_mode(MODE_IMU);
+    _imu.change_fusion_mode(MODE_NDOF);
     char data[]= {0,0,0};
     if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0)
         return ERROR_RX_P;
@@ -53,7 +54,8 @@
             break;
     }
     _xya.a=_angle;
-    xya->a=_xya.a;
+    //xya->a=_xya.a;
+    xya->a=_euler.h;
 }
 
 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
@@ -100,4 +102,4 @@
     wait(R_WAIT);
     _ry_n=data[1];
     //printf("%d %d %d %d\n",_rx_p,_rx_n,_ry_p,_ry_n);
-}
\ No newline at end of file
+}