Diff: main.cpp

Revision:

1:8966a48ce8d5

Parent:

0:96d6eb224379

Child:

3:c58eb4be51de

--- a/main.cpp	Tue Mar 01 18:20:45 2016 +0000
+++ b/main.cpp	Tue Mar 01 19:26:32 2016 +0000
@@ -1,10 +1,5 @@
 #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
@@ -22,10 +17,6 @@
 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;
@@ -37,64 +28,14 @@
 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);
+        //loc.get_angle(&xya);
+        loc.get_xy(&xya);
+        pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
     }
     /*if (imu.chip_ready()==0) {
         //pc.printf("BNO055 is NOT available!\n");