Edwin Cho / Mbed 2 deprecated DEMO3

Localization

Dependencies:   BNO055_fusion mbed

Diff: main.cpp

Revision:
11:9518f8285906
Parent:
10:cf44f4387bc7
Child:
12:3b26fcc7da7e
--- a/main.cpp	Wed Mar 23 16:20:53 2016 +0000
+++ b/main.cpp	Wed Mar 23 16:22:53 2016 +0000
@@ -81,7 +81,7 @@
     float s = 0;
     int diff = 0;
     diff = 180-wrap(angle);
-    if(abs(xya.a-angle)<=5)
+    if(abs(wrap(xya.a+diff)-180)<=5)
         s=SPEED_TURN_MIN;
     else
         s=(SPEED_TURN_MAX*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;