Localization

Dependencies:   BNO055_fusion mbed

Revision:
8:b36be08c44f8
Parent:
7:d6dca30f7959
Child:
9:4fc5d70c3bab
--- a/main.cpp	Thu Mar 17 23:20:22 2016 +0000
+++ b/main.cpp	Wed Mar 23 06:55:39 2016 +0000
@@ -7,8 +7,8 @@
 #define SPEED_FB_MIN    0.15
 #define SPEED_FB_MAX    0.50
 
-//Serial pc(p13, p14);
-Serial pc(USBTX, USBRX);
+Serial pc(p13, p14);
+//Serial pc(USBTX, USBRX);
 
 Watchdog wdt;
 
@@ -51,7 +51,7 @@
     motor2F=0;
     motor2B=0;
     //pc.printf("Initialized Localization: %d\n",loc.init());
-    //t.attach(&send,1);
+    t.attach(&send,1);
     sw1.mode(PullUp);
     sw2.mode(PullUp);
     sw3.mode(PullUp);
@@ -66,9 +66,10 @@
         led3=!sw3;
         led4=!sw4;
         //loc.get_angle(&xya);
-        //loc.get_xy(&xya);
-        loc.get_raw_xy();
-        pc.printf("X: %3d\tY: %3d\tP: %3d\n",loc._rx_n,xya.y,xya.a);
+        loc.get_xy(&xya);
+        //loc.get_raw_xy();
+        //pc.printf("%c%c%c%c\n",xya.x,xya.y,xya.a/10,xya.a%10);
+        //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
         /*if(loc._rx_n>20) {
             dir1=1;
             dir2=1;
@@ -125,7 +126,7 @@
 
 void send()
 {
-    pc.printf("%c%c%c%c\n",(char)loc._rx_n,(char)loc._ry_n,xya.a/10,xya.a%10);
+    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
 }
 
 void turn(int angle)