Edwin Cho / Mbed 2 deprecated DEMO3

Localization

Dependencies:   BNO055_fusion mbed

main.cpp

Committer:
12104404
Date:
2016-03-06
Revision:
5:c89308dc1827
Parent:
4:d70375cfa533
Child:
6:0602a9e8118b

File content as of revision 5:c89308dc1827:

#include "LOCALIZE.h"

//Serial pc(p13, p14);
Serial pc(USBTX, USBRX);

I2C i2c1(p28, p27);
I2C i2c2(p9, p10);
LOCALIZE loc(i2c1, i2c2, p26);
LOCALIZE_xya xya;

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

int main()
{
    pc.baud(9600);
    pc.printf("Initialized Localization: %d\n",loc.init());
    while(1) {
        //loc.get_angle(&xya);
        loc.get_xy(&xya);
        pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
        //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
        //pc.printf("%c%c%c%c\n",xya.x,xya.y,xya.a/10,xya.a%10);
    }
}