Localization

Dependencies:   BNO055_fusion mbed

Revision:
7:d6dca30f7959
Parent:
6:0602a9e8118b
Child:
8:b36be08c44f8
--- a/main.cpp	Wed Mar 16 22:05:23 2016 +0000
+++ b/main.cpp	Thu Mar 17 23:20:22 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,24 +51,24 @@
     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);
     sw4.mode(PullUp);
-    while(abs(xya.a-180)>5) {
+    /*while(abs(xya.a-180)>5) {
         loc.get_angle(&xya);
         turn(180);
-    }
+    }*/
     while(1) {
         led1=!sw1;
         led2=!sw2;
         led3=!sw3;
         led4=!sw4;
-        loc.get_angle(&xya);
+        //loc.get_angle(&xya);
         //loc.get_xy(&xya);
         loc.get_raw_xy();
-        //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
+        pc.printf("X: %3d\tY: %3d\tP: %3d\n",loc._rx_n,xya.y,xya.a);
         /*if(loc._rx_n>20) {
             dir1=1;
             dir2=1;
@@ -76,7 +76,7 @@
             dir1=0;
             dir2=0;
         }*/
-
+/*
         if((!sw1==1 && !sw2==1) && flag) {
             motor1F=0.3;
             motor1B=0.3;
@@ -118,7 +118,7 @@
             dir1=1;
             dir2=1;
         }
-
+*/
         wdt.kick();
     }
 }