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);
+//    }
+}