![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 0:96d6eb224379
- Child:
- 1:8966a48ce8d5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Mar 01 18:20:45 2016 +0000 @@ -0,0 +1,121 @@ +#include "LOCALIZE.h" + +#define RANGE_ADDR 0x04 +#define FRAME_H 180 +#define FRAME_W 150 +#define RANGE_X_OFF 7 +#define RANGE_Y_OFF 6 +#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); + +//BNO055_ID_INF_TypeDef bno055_id_inf; +//BNO055_EULER_TypeDef euler; +//BNO055_GRAVITY_TypeDef gravity; +//BNO055_LIN_ACC_TypeDef accel; +int range_px=-1; +int range_py=-1; +int range_x=-1; +int range_y=-1; +int p_x=-1; +int p_y=-1; +int p_angle=0; +int err=-1; +int range_jmps_y=RANGE_JMPS; +int range_jmps_x=RANGE_JMPS; + +//void x_y() +//{ +// err=0; +// if(p_angle<RANGE_ERROR || p_angle>360-RANGE_ERROR) { +// p_x=range_x; +// p_y=range_y; +// } else if(abs(p_angle-90)<RANGE_ERROR) { +// p_x=range_y; +// p_y=FRAME_H-range_x-RANGE_X_OFF; +// } else if(abs(p_angle-180)<RANGE_ERROR) { +// p_x=FRAME_W-range_x-RANGE_X_OFF; +// p_y=FRAME_H-range_y-RANGE_Y_OFF; +// } else if(abs(p_angle-270)<RANGE_ERROR) { +// p_x=FRAME_W-range_y-RANGE_Y_OFF; +// p_y=range_x; +// } else { +// err=-1; +// p_x=range_x; +// p_y=range_y; +// } +// if(p_x>=FRAME_W) +// p_x=FRAME_W; +// if(p_y>=FRAME_H) +// p_y=FRAME_H; +//} +// +//void jump() +//{ +// if(abs(range_x-range_px)<RANGE_DIFF || range_jmps_x>RANGE_JMPS) { +// if(range_x>0) { +// range_px=range_x; +// } else { +// range_x=range_px; +// } +// range_jmps_x=0; +// } else { +// range_jmps_x++; +// } +// +// if(abs(range_y-range_py)<RANGE_DIFF || range_jmps_y>RANGE_JMPS) { +// if(range_y>0) { +// range_py=range_y; +// } else { +// range_y=range_py; +// } +// range_jmps_y=0; +// } else { +// range_jmps_y++; +// } +//} + +int main() +{ + pc.baud(9600); + pc.printf("IMU INIT %d\n",loc.init()); + while(1) { + loc.get_angle(&xya); + pc.printf("Angle: %d\n",xya.a); + } + /*if (imu.chip_ready()==0) { + //pc.printf("BNO055 is NOT available!\n"); + }*/ + //imu.set_mounting_position(MT_P0); + //imu.change_fusion_mode(MODE_IMU); + //p_x=location.x; +// while(1) { +// char data[]= {0,0,0}; +// i2c1.read(RANGE_ADDR<<1, data, 3); +// wait(0.05); +// range_y=data[1]; +// i2c2.read(RANGE_ADDR<<1, data, 3); +// wait(0.05); +// //imu.get_linear_accel(&accel); +// range_x=data[1]; +// //p_angle=get_angle(); +// //jump(); +// //x_y(); +// //pc.printf("X: %3d\tY: %3d\tP: %3d\tERROR:%3d\n",p_x,p_y,p_angle,err); +// pc.printf("%c%c%c%c\n",p_x,p_y,p_angle/10,p_angle%10); +// wait(0.5); +// } +}