blue mbed code for the BNO055 imu from adafruit
Dependencies: BNO055 MODSERIAL mbed
Fork of bmbed_lidar_belt by
Diff: main.cpp
- Revision:
- 15:d6edd100d7fe
- Parent:
- 14:8620dceb3ba2
- Child:
- 16:70fce771d828
--- a/main.cpp Sat Oct 17 19:02:41 2015 +0000 +++ b/main.cpp Sat Oct 17 19:30:41 2015 +0000 @@ -77,7 +77,8 @@ bool newline_sent = false; float potValue(){ - float pot = potentiometer.read(); + //float pot = potentiometer.read(); + float pot = 0.25; if(pot <= 0.5) return 2*pot; else @@ -173,13 +174,17 @@ //calibrate down sensor int down_cal = 0; + int down_cal2 = 0; float cospi = 0; //"cosine of initial pitch" unsigned int i = 0; int count = 0; //for calibration int count2 = 0;//for averaging + int count2_2 = 0; //for averaging second down sensor int differenceAvgSum = 0; + int differenceAvgSum2 = 0; int moving_ave[5] = {0}; + int moving_ave2[5] = {0}; while (1) { for(int k=0; k<5; k++) { char receiveData[3] = {0}; @@ -201,6 +206,9 @@ pulses[k] = 1; intensity[k] = 0; } + //for(int z=0;z<4;z++) + // pc.printf("angle: %f radius %d is %f ",angle[k],z,radius[z]); + //pc.printf("\n"); if(distance > 0 && distance < radius[0]) { pulses[k] = 5; intensity[k] = 7; @@ -221,19 +229,60 @@ } //find UP distance +// char receiveData2[3] = {0}; +// sensor.write(addresses[5], sendData, 1); +// i = 0; +// while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){ +// i++;} +// int distance2 = (receiveData2[0]<<8 )+ receiveData2[1]; +// if(distance2 >= UP_MIN && distance2 < UP_MAX) { +// pulses[5] = 5; ///TODO WAS: 5 +// intensity[5] = 7; ///TODO: 7 +// } else { +// pulses[5] = 1; +// intensity[5] = 0; +// } + + //NEW down sensor: char receiveData2[3] = {0}; sensor.write(addresses[5], sendData, 1); i = 0; while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){ i++;} int distance2 = (receiveData2[0]<<8 )+ receiveData2[1]; - if(distance2 >= UP_MIN && distance2 < UP_MAX) { - pulses[5] = 5; ///TODO WAS: 5 - intensity[5] = 7; ///TODO: 7 - } else { - pulses[5] = 1; - intensity[5] = 0; + if(count > 50) { //calibration over + //get allowableX and adjusted down_cal from IMU + imu.get_angles(); + float pitch = imu.euler.pitch; + float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0); + float allowX = ALLOW_HEIGHT/cosp; + float new_down_cal = ((float)down_cal2)*cospi/cosp; + + //use moving average to find deltaX + int difference = abs(new_down_cal - distance2); + differenceAvgSum2 = differenceAvgSum2 - moving_ave2[count2_2]; + moving_ave2[count2_2] = difference; + differenceAvgSum2 = differenceAvgSum2 + difference; + count2_2 = count2_2 + 1; + int ave = (int)(differenceAvgSum2/5); + + //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) { + pulses[5] = 5; + intensity[5] = 7; + } else { + pulses[5] = 1; + intensity[5] = 0; + } + + if(count2_2 >4) { + count2 = 0; + } } + else + down_cal2 = distance2; //find DOWN distance char receiveData3[3] = {0}; @@ -242,7 +291,7 @@ while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){ i++;} int distance3 = (receiveData3[0]<<8 )+ receiveData3[1]; - if(count > 100) { //calibration over + if(count > 50) { //calibration over //get allowableX and adjusted down_cal from IMU imu.get_angles(); float pitch = imu.euler.pitch;