Edwin Cho / Mbed 2 deprecated DEMO3

Localization

Dependencies:   BNO055_fusion mbed

main.cpp@1:8966a48ce8d5, 2016-03-01 (annotated)

Committer:
12104404
Date:
Tue Mar 01 19:26:32 2016 +0000
Revision:
1:8966a48ce8d5
Parent:
0:96d6eb224379
Child:
3:c58eb4be51de
XY ANGLE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12104404 0:96d6eb224379 1 #include "LOCALIZE.h"
12104404 0:96d6eb224379 2
12104404 0:96d6eb224379 3 #define RANGE_ERROR 14
12104404 0:96d6eb224379 4 #define RANGE_DIFF 10
12104404 0:96d6eb224379 5 #define RANGE_JMPS 4
12104404 0:96d6eb224379 6
12104404 0:96d6eb224379 7 //Serial pc(p13, p14);
12104404 0:96d6eb224379 8 Serial pc(USBTX, USBRX);
12104404 0:96d6eb224379 9
12104404 0:96d6eb224379 10 I2C i2c1(p28, p27);
12104404 0:96d6eb224379 11 I2C i2c2(p9, p10);
12104404 0:96d6eb224379 12 LOCALIZE loc(i2c1, i2c2, p26);
12104404 0:96d6eb224379 13 LOCALIZE_xya xya;
12104404 0:96d6eb224379 14
12104404 0:96d6eb224379 15 DigitalOut led1(LED1);
12104404 0:96d6eb224379 16 DigitalOut led2(LED2);
12104404 0:96d6eb224379 17 DigitalOut led3(LED3);
12104404 0:96d6eb224379 18 DigitalOut led4(LED4);
12104404 0:96d6eb224379 19
12104404 0:96d6eb224379 20 int range_px=-1;
12104404 0:96d6eb224379 21 int range_py=-1;
12104404 0:96d6eb224379 22 int range_x=-1;
12104404 0:96d6eb224379 23 int range_y=-1;
12104404 0:96d6eb224379 24 int p_x=-1;
12104404 0:96d6eb224379 25 int p_y=-1;
12104404 0:96d6eb224379 26 int p_angle=0;
12104404 0:96d6eb224379 27 int err=-1;
12104404 0:96d6eb224379 28 int range_jmps_y=RANGE_JMPS;
12104404 0:96d6eb224379 29 int range_jmps_x=RANGE_JMPS;
12104404 0:96d6eb224379 30
12104404 0:96d6eb224379 31 int main()
12104404 0:96d6eb224379 32 {
12104404 0:96d6eb224379 33 pc.baud(9600);
12104404 0:96d6eb224379 34 pc.printf("IMU INIT %d\n",loc.init());
12104404 0:96d6eb224379 35 while(1) {
12104404 1:8966a48ce8d5 36 //loc.get_angle(&xya);
12104404 1:8966a48ce8d5 37 loc.get_xy(&xya);
12104404 1:8966a48ce8d5 38 pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
12104404 0:96d6eb224379 39 }
12104404 0:96d6eb224379 40 /*if (imu.chip_ready()==0) {
12104404 0:96d6eb224379 41 //pc.printf("BNO055 is NOT available!\n");
12104404 0:96d6eb224379 42 }*/
12104404 0:96d6eb224379 43 //imu.set_mounting_position(MT_P0);
12104404 0:96d6eb224379 44 //imu.change_fusion_mode(MODE_IMU);
12104404 0:96d6eb224379 45 //p_x=location.x;
12104404 0:96d6eb224379 46 // while(1) {
12104404 0:96d6eb224379 47 // char data[]= {0,0,0};
12104404 0:96d6eb224379 48 // i2c1.read(RANGE_ADDR<<1, data, 3);
12104404 0:96d6eb224379 49 // wait(0.05);
12104404 0:96d6eb224379 50 // range_y=data[1];
12104404 0:96d6eb224379 51 // i2c2.read(RANGE_ADDR<<1, data, 3);
12104404 0:96d6eb224379 52 // wait(0.05);
12104404 0:96d6eb224379 53 // //imu.get_linear_accel(&accel);
12104404 0:96d6eb224379 54 // range_x=data[1];
12104404 0:96d6eb224379 55 // //p_angle=get_angle();
12104404 0:96d6eb224379 56 // //jump();
12104404 0:96d6eb224379 57 // //x_y();
12104404 0:96d6eb224379 58 // //pc.printf("X: %3d\tY: %3d\tP: %3d\tERROR:%3d\n",p_x,p_y,p_angle,err);
12104404 0:96d6eb224379 59 // pc.printf("%c%c%c%c\n",p_x,p_y,p_angle/10,p_angle%10);
12104404 0:96d6eb224379 60 // wait(0.5);
12104404 0:96d6eb224379 61 // }
12104404 0:96d6eb224379 62 }