Localization
Dependencies: BNO055_fusion mbed
Diff: main.cpp
- 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(); } }