Edwin Cho / Mbed 2 deprecated DEMO3

Localization

Dependencies:   BNO055_fusion mbed

main.cpp

Committer:
12104404
Date:
2016-03-03
Revision:
3:c58eb4be51de
Parent:
1:8966a48ce8d5
Child:
4:d70375cfa533

File content as of revision 3:c58eb4be51de:

#include "LOCALIZE.h"

#define RANGE_ERROR 14
#define RANGE_DIFF  10
#define RANGE_JMPS  4

//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("IMU INIT %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);
    }
}