blue mbed code for the BNO055 imu from adafruit
Dependencies: BNO055 MODSERIAL mbed
Fork of bmbed_lidar_belt by
Diff: main.cpp
- Revision:
- 13:b22fd9c4fbb4
- Parent:
- 12:6deb3b41c9e3
- Child:
- 14:8620dceb3ba2
--- a/main.cpp Fri Oct 16 19:42:06 2015 +0000 +++ b/main.cpp Fri Oct 16 20:10:10 2015 +0000 @@ -2,33 +2,33 @@ #include "MODSERIAL.h" #include <math.h> #include "BNO055.h" +//for calculating allowable change in height for the down lidar +#define DOWN_ANGLE -5.6 +#define ALLOW_HEIGHT 120 + +//Up Delta +#define UP_MIN 500 +#define UP_MAX 1100 //Initial LONG Range Settings -#define RANGE_01 750 -#define RANGE_02 1100 -#define RANGE_03 1700 -#define RANGE_04 2750 -#define UP_MIN 600 -#define UP_MAX 1300 -#define DOWN_DIFF 500 +//#define RANGE_01 750 +//#define RANGE_02 1100 +//#define RANGE_03 1700 +//#define RANGE_04 2750 +//#define UP_MIN 600 +//#define UP_MAX 1300 // Mid range settings -//#define RANGE_01 450 -//#define RANGE_02 750 -//#define RANGE_03 1150 -//#define RANGE_04 1550 -//#define UP_MIN 600 -//#define UP_MAX 1300 -//#define DOWN_DIFF 260 +#define RANGE_01 450 +#define RANGE_02 750 +#define RANGE_03 1150 +#define RANGE_04 1550 // Short range settings //#define RANGE_01 400 //#define RANGE_02 650 //#define RANGE_03 1000 //#define RANGE_04 1350 -//#define UP_MIN 600 -//#define UP_MAX 1300 -//#define DOWN_DIFF 260 #define PC_BAUD 9600 @@ -47,9 +47,7 @@ MODSERIAL bt(TX_PIN, RX_PIN); MODSERIAL pc(USBTX,USBRX); -//for calculating allowable change in height for the down lidar -float allowHeight = 120; //120 mm -float downAngle = -5.6; + //for calibrating IMU //for IMU1: //char cal_vals[22] = {255, 255, 220, 255, 13, 0, 83, 255, 36, 1, 80, 0, 253, 255, 0, 0, 1, 0, 232, 3, 235, 2}; @@ -181,8 +179,8 @@ i++;} int distance2 = (receiveData2[0]<<8 )+ receiveData2[1]; if(distance2 >= UP_MIN && distance2 < UP_MAX) { - pulses[5] = 1; ///TODO WAS: 5 - intensity[5] = 0; ///TODO: 7 + pulses[5] = 5; ///TODO WAS: 5 + intensity[5] = 7; ///TODO: 7 } else { pulses[5] = 1; intensity[5] = 0; @@ -199,8 +197,8 @@ //get allowableX and adjusted down_cal from IMU imu.get_angles(); float pitch = imu.euler.pitch; - float cosp = cos((pitch-downAngle) * PI/180.0); - float allowX = allowHeight/cosp; + float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0); + float allowX = ALLOW_HEIGHT/cosp; float new_down_cal = ((float)down_cal)*cospi/cosp; //use moving average to find deltaX @@ -211,7 +209,7 @@ count2 = count2 + 1; int ave = (int)(differenceAvgSum/5); - //pc.printf("distance: %d\tallowableX: %f\tdistance: %d\tpitch: %f\tdowncal: %d\tnewdowncal: %f\r\n",ave,allowX,distance3,pitch-downAngle,down_cal,new_down_cal); + //pc.printf("distance: %d\tallowableX: %f\tdistance: %d\tpitch: %f\tdowncal: %d\tnewdowncal: %f\r\n",ave,allowX,distance3,pitch-DOWN_ANGLE,down_cal,new_down_cal); //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3); if(ave >= allowX) { @@ -230,7 +228,7 @@ down_cal = distance3; imu.get_angles(); float pitch = imu.euler.pitch; - cospi = cos((pitch-downAngle) * PI/180.0); + cospi = cos((pitch-DOWN_ANGLE) * PI/180.0); count = count+1; } //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",6,pulses[6],intensity[6]);