Edwin Cho / Mbed 2 deprecated DEMO3

Localization

Dependencies:   BNO055_fusion mbed

Diff: main.cpp

Revision:
16:d6f15a13c3aa
Parent:
15:7729da55873a
Child:
17:2f89826b5679
--- a/main.cpp	Thu Mar 24 07:20:51 2016 +0000
+++ b/main.cpp	Sun Mar 27 02:39:07 2016 +0000
@@ -12,7 +12,7 @@
 
 I2C i2c1(p28, p27);
 I2C i2c2(p9, p10);
-LOCALIZE loc(i2c1, i2c2, p26, p20, p19, p18, p17);
+LOCALIZE loc(i2c1, i2c2, p26, p8, p7, p6, p5);
 LOCALIZE_xya xya;
 LOCOMOTION motion(p21, p22, p23, p24, p15, p16);
 
@@ -30,11 +30,10 @@
     //pc.printf("Initialized Localization: %d\n",loc.init());
     t.attach(&send,1);
     while(1) {
-        motion.setAngle(0,0,5,ANGLE_TURN);
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
+        motion.setAngle(0,xya.a,2,ANGLE_TURN);
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
-        //setAngle(0);
         wdt.kick();
     }
 }