Antoine Laurens / Mbed 2 deprecated DEMO3

mechatronics drive

Dependencies:   BNO055_fusion mbed

Fork of DEMO3 by Edwin Cho

main.cpp@0:96d6eb224379, 2016-03-01 (annotated)

Committer:
12104404
Date:
Tue Mar 01 18:20:45 2016 +0000
Revision:
0:96d6eb224379
Child:
1:8966a48ce8d5
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_ADDR 0x04
12104404 0:96d6eb224379 4 #define FRAME_H 180
12104404 0:96d6eb224379 5 #define FRAME_W 150
12104404 0:96d6eb224379 6 #define RANGE_X_OFF 7
12104404 0:96d6eb224379 7 #define RANGE_Y_OFF 6
12104404 0:96d6eb224379 8 #define RANGE_ERROR 14
12104404 0:96d6eb224379 9 #define RANGE_DIFF 10
12104404 0:96d6eb224379 10 #define RANGE_JMPS 4
12104404 0:96d6eb224379 11
12104404 0:96d6eb224379 12 //Serial pc(p13, p14);
12104404 0:96d6eb224379 13 Serial pc(USBTX, USBRX);
12104404 0:96d6eb224379 14
12104404 0:96d6eb224379 15 I2C i2c1(p28, p27);
12104404 0:96d6eb224379 16 I2C i2c2(p9, p10);
12104404 0:96d6eb224379 17 LOCALIZE loc(i2c1, i2c2, p26);
12104404 0:96d6eb224379 18 LOCALIZE_xya xya;
12104404 0:96d6eb224379 19
12104404 0:96d6eb224379 20 DigitalOut led1(LED1);
12104404 0:96d6eb224379 21 DigitalOut led2(LED2);
12104404 0:96d6eb224379 22 DigitalOut led3(LED3);
12104404 0:96d6eb224379 23 DigitalOut led4(LED4);
12104404 0:96d6eb224379 24
12104404 0:96d6eb224379 25 //BNO055_ID_INF_TypeDef bno055_id_inf;
12104404 0:96d6eb224379 26 //BNO055_EULER_TypeDef euler;
12104404 0:96d6eb224379 27 //BNO055_GRAVITY_TypeDef gravity;
12104404 0:96d6eb224379 28 //BNO055_LIN_ACC_TypeDef accel;
12104404 0:96d6eb224379 29 int range_px=-1;
12104404 0:96d6eb224379 30 int range_py=-1;
12104404 0:96d6eb224379 31 int range_x=-1;
12104404 0:96d6eb224379 32 int range_y=-1;
12104404 0:96d6eb224379 33 int p_x=-1;
12104404 0:96d6eb224379 34 int p_y=-1;
12104404 0:96d6eb224379 35 int p_angle=0;
12104404 0:96d6eb224379 36 int err=-1;
12104404 0:96d6eb224379 37 int range_jmps_y=RANGE_JMPS;
12104404 0:96d6eb224379 38 int range_jmps_x=RANGE_JMPS;
12104404 0:96d6eb224379 39
12104404 0:96d6eb224379 40 //void x_y()
12104404 0:96d6eb224379 41 //{
12104404 0:96d6eb224379 42 // err=0;
12104404 0:96d6eb224379 43 // if(p_angle<RANGE_ERROR || p_angle>360-RANGE_ERROR) {
12104404 0:96d6eb224379 44 // p_x=range_x;
12104404 0:96d6eb224379 45 // p_y=range_y;
12104404 0:96d6eb224379 46 // } else if(abs(p_angle-90)<RANGE_ERROR) {
12104404 0:96d6eb224379 47 // p_x=range_y;
12104404 0:96d6eb224379 48 // p_y=FRAME_H-range_x-RANGE_X_OFF;
12104404 0:96d6eb224379 49 // } else if(abs(p_angle-180)<RANGE_ERROR) {
12104404 0:96d6eb224379 50 // p_x=FRAME_W-range_x-RANGE_X_OFF;
12104404 0:96d6eb224379 51 // p_y=FRAME_H-range_y-RANGE_Y_OFF;
12104404 0:96d6eb224379 52 // } else if(abs(p_angle-270)<RANGE_ERROR) {
12104404 0:96d6eb224379 53 // p_x=FRAME_W-range_y-RANGE_Y_OFF;
12104404 0:96d6eb224379 54 // p_y=range_x;
12104404 0:96d6eb224379 55 // } else {
12104404 0:96d6eb224379 56 // err=-1;
12104404 0:96d6eb224379 57 // p_x=range_x;
12104404 0:96d6eb224379 58 // p_y=range_y;
12104404 0:96d6eb224379 59 // }
12104404 0:96d6eb224379 60 // if(p_x>=FRAME_W)
12104404 0:96d6eb224379 61 // p_x=FRAME_W;
12104404 0:96d6eb224379 62 // if(p_y>=FRAME_H)
12104404 0:96d6eb224379 63 // p_y=FRAME_H;
12104404 0:96d6eb224379 64 //}
12104404 0:96d6eb224379 65 //
12104404 0:96d6eb224379 66 //void jump()
12104404 0:96d6eb224379 67 //{
12104404 0:96d6eb224379 68 // if(abs(range_x-range_px)<RANGE_DIFF || range_jmps_x>RANGE_JMPS) {
12104404 0:96d6eb224379 69 // if(range_x>0) {
12104404 0:96d6eb224379 70 // range_px=range_x;
12104404 0:96d6eb224379 71 // } else {
12104404 0:96d6eb224379 72 // range_x=range_px;
12104404 0:96d6eb224379 73 // }
12104404 0:96d6eb224379 74 // range_jmps_x=0;
12104404 0:96d6eb224379 75 // } else {
12104404 0:96d6eb224379 76 // range_jmps_x++;
12104404 0:96d6eb224379 77 // }
12104404 0:96d6eb224379 78 //
12104404 0:96d6eb224379 79 // if(abs(range_y-range_py)<RANGE_DIFF || range_jmps_y>RANGE_JMPS) {
12104404 0:96d6eb224379 80 // if(range_y>0) {
12104404 0:96d6eb224379 81 // range_py=range_y;
12104404 0:96d6eb224379 82 // } else {
12104404 0:96d6eb224379 83 // range_y=range_py;
12104404 0:96d6eb224379 84 // }
12104404 0:96d6eb224379 85 // range_jmps_y=0;
12104404 0:96d6eb224379 86 // } else {
12104404 0:96d6eb224379 87 // range_jmps_y++;
12104404 0:96d6eb224379 88 // }
12104404 0:96d6eb224379 89 //}
12104404 0:96d6eb224379 90
12104404 0:96d6eb224379 91 int main()
12104404 0:96d6eb224379 92 {
12104404 0:96d6eb224379 93 pc.baud(9600);
12104404 0:96d6eb224379 94 pc.printf("IMU INIT %d\n",loc.init());
12104404 0:96d6eb224379 95 while(1) {
12104404 0:96d6eb224379 96 loc.get_angle(&xya);
12104404 0:96d6eb224379 97 pc.printf("Angle: %d\n",xya.a);
12104404 0:96d6eb224379 98 }
12104404 0:96d6eb224379 99 /*if (imu.chip_ready()==0) {
12104404 0:96d6eb224379 100 //pc.printf("BNO055 is NOT available!\n");
12104404 0:96d6eb224379 101 }*/
12104404 0:96d6eb224379 102 //imu.set_mounting_position(MT_P0);
12104404 0:96d6eb224379 103 //imu.change_fusion_mode(MODE_IMU);
12104404 0:96d6eb224379 104 //p_x=location.x;
12104404 0:96d6eb224379 105 // while(1) {
12104404 0:96d6eb224379 106 // char data[]= {0,0,0};
12104404 0:96d6eb224379 107 // i2c1.read(RANGE_ADDR<<1, data, 3);
12104404 0:96d6eb224379 108 // wait(0.05);
12104404 0:96d6eb224379 109 // range_y=data[1];
12104404 0:96d6eb224379 110 // i2c2.read(RANGE_ADDR<<1, data, 3);
12104404 0:96d6eb224379 111 // wait(0.05);
12104404 0:96d6eb224379 112 // //imu.get_linear_accel(&accel);
12104404 0:96d6eb224379 113 // range_x=data[1];
12104404 0:96d6eb224379 114 // //p_angle=get_angle();
12104404 0:96d6eb224379 115 // //jump();
12104404 0:96d6eb224379 116 // //x_y();
12104404 0:96d6eb224379 117 // //pc.printf("X: %3d\tY: %3d\tP: %3d\tERROR:%3d\n",p_x,p_y,p_angle,err);
12104404 0:96d6eb224379 118 // pc.printf("%c%c%c%c\n",p_x,p_y,p_angle/10,p_angle%10);
12104404 0:96d6eb224379 119 // wait(0.5);
12104404 0:96d6eb224379 120 // }
12104404 0:96d6eb224379 121 }