Edwin Cho / Mbed 2 deprecated DEMO3

Localization

Dependencies:   BNO055_fusion mbed

Diff: main.cpp

Revision:
12:3b26fcc7da7e
Parent:
11:9518f8285906
Child:
13:c62f975dfcfe
--- a/main.cpp	Wed Mar 23 16:22:53 2016 +0000
+++ b/main.cpp	Wed Mar 23 16:25:48 2016 +0000
@@ -66,7 +66,7 @@
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
-        setAngle(20);
+        setAngle(170);
         wdt.kick();
     }
 }
@@ -84,7 +84,7 @@
     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;
+        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
     motor1F=s;
     motor1B=s;
     motor2F=s;