blue mbed code for the BNO055 imu from adafruit
Dependencies: BNO055 MODSERIAL mbed
Fork of bmbed_lidar_belt by
Diff: main.cpp
- Revision:
- 17:32b83c5787fc
- Parent:
- 16:70fce771d828
- Child:
- 19:01d091b38d61
diff -r 70fce771d828 -r 32b83c5787fc main.cpp --- a/main.cpp Sat Oct 17 19:55:27 2015 +0000 +++ b/main.cpp Wed Nov 25 23:23:26 2015 +0000 @@ -2,115 +2,28 @@ #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 - -float MIN_MIN_A=200.0; //min side range -float MAX_MIN_A=500.0; //min side range -float MIN_MAX_A=400.0; //max side range -float MAX_MAX_A=1200.0; //max side range - -float MIN_MIN_B=400.0; //min front range -float MAX_MIN_B=1000.0; //min front range -float MIN_MAX_B=1200.0; //max front range -float MAX_MAX_B=2750.0; //max front range - -//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 - -// Mid range settings -#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 PC_BAUD 9600 -#define BT_BAUD 115200 -#define TX_PIN p13 -#define RX_PIN p14 -#define SDA_PIN p9 //SDA pin on LPC1768 -#define SCL_PIN p10 //SCL pin on LPC1768 +//#define BT_BAUD 115200 + +//#define TX_PIN p13 +//#define RX_PIN p14 #define IMU_SDA p28 #define IMU_SCL p27 #define PI 3.14159265 -float angle[5] = {PI, 3*PI/4, PI/2, PI/4, 0}; //relates lidar number to its angle in radians - BNO055 imu(p28,p27); -I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1 -MODSERIAL bt(TX_PIN, RX_PIN); +//MODSERIAL bt(TX_PIN, RX_PIN); MODSERIAL pc(USBTX,USBRX); -AnalogIn potentiometer(p18); //doesn't exist yet - //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}; -//for IMU2: -char cal_vals[22] = {231, 255, 253, 255, 8, 0, 43, 255, 31, 1, 221, 255, 0, 0, 254, 255, 2, 0, 232, 3, 210, 2}; - - -//for encoder: -//need to set pins to DigitalIn with internal pullup resistors -//DigitalIn mySwitch(p21); -//mySwitch.mode(PullUp); +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}; bool newline_detected = false; bool newline_sent = false; -float potValue(){ - //float pot = potentiometer.read(); - float pot = ((float)((char)pc.getc()) - '0')/18.0; - if(pot <= 0.5) - return 2*pot; - else - return 2*(1-pot); //if pot = 1, return 0. if pot = 0.5, return 1 -} - -void angleToRadius(float* radius,float angle){ - //a is horizontal semi-axis - //b is vertical semi-axis - //scale between MIN_A, MAX_A, MIN_B, MAX_B - //using pot value - float pot = potValue(); - //define new min_a scaled between MIN_MIN_A and MAX_MIN_A - float min_a = MIN_MIN_A + (MAX_MIN_A - MIN_MIN_A)*pot; - //define new max_a scaled between MIN_MAX_A and MAX_MAX_A - float max_a = MIN_MAX_A + (MAX_MAX_A - MIN_MAX_A)*pot; - //define new min_b between MIN_MIN_B and MAX_MIN_B - float min_b = MIN_MIN_B + (MAX_MIN_B - MIN_MIN_B)*pot; - //define new max_b scaled between MIN_MAX_B and MAX_MAX_B - float max_b = MIN_MAX_B + (MAX_MAX_B - MIN_MAX_B)*pot; - - float scale[4] = {0, 0.3, 0.6,1.0}; - for(int i=0;i<4;i++){ - //scale a and be to be somewhere between the min and max based on i - // i = 0 -- min - // i = 5 -- max - float a = min_a + (max_a - min_a)*scale[i]; - float b = min_b + (max_b - min_b)*scale[i]; - radius[i] = a*b/sqrt(pow(a,2)*pow(sin(angle),2) + pow(b,2)*pow(cos(angle),2)); - } -} - void setCal(){ imu.write_calibration_data(); } @@ -140,9 +53,9 @@ int main() { pc.baud(PC_BAUD); - bt.baud(BT_BAUD); +// bt.baud(BT_BAUD); pc.attach(&rxCallback, MODSERIAL::RxIrq); - bt.attach(&txCallback, MODSERIAL::TxIrq); +// bt.attach(&txCallback, MODSERIAL::TxIrq); //set up IMU imu.reset(); @@ -154,200 +67,22 @@ imu.get_calib(); } - sensor.frequency(100000); - char sendData[1] = {0x00}; - int addresses[7]; - addresses[0] = 0x60; //0x60 - addresses[1] = 0x64; //0x64 - addresses[2] = 0x68; //middle - addresses[3] = 0x6C; - addresses[4] = 0x70; - addresses[5] = 0x80; //up - addresses[6] = 0x84; //down - - uint8_t pulses[7] = {0}; - uint8_t intensity[7] = {0}; - - char btData[12] = {'a','b','c','d','e','f','g','\n','\0'}; - - //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}; - if(sensor.write(addresses[k], sendData, 1)){ - //pc.printf("writing to sensor %d failed\n", k); - } - //write ---> 0 on success, 1 on failure - i = 0; - while(sensor.read(addresses[k], receiveData, 3) && i < 10) { - i++; - //pc.printf("reading from sensor %d failed\n",k); - } - //while(!twi_master_transfer(addresses[k], sendData, 1, TWI_ISSUE_STOP)){;} - //while(!twi_master_transfer(addresses[k] + 1, receiveData, 3, TWI_ISSUE_STOP)){;} - int distance = ((int)receiveData[0]<<8 )+ (int)receiveData[1]; - float radius[4] = {0}; - angleToRadius(radius, angle[k]); - if(distance == 0){ - 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; - } else if(distance >= radius[0] && distance < radius[1]) { - pulses[k] = 4; - intensity[k] = 6; - } else if(distance >= radius[1] && distance < radius[2]) { - pulses[k] = 3; - intensity[k] = 5; - } else if(distance >= radius[2] && distance < radius[3]) { - pulses[k] = 2; - intensity[k] = 2; - } else if(distance >= radius[3]) { - pulses[k] = 1; - intensity[k] = 0; - } - //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",k,pulses[k],intensity[k]); - } - - //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; -// } +// char btData[12] = {'a','b','c','d','e','f','g','\n','\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(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}; - i = 0; - sensor.write(addresses[6], sendData, 1); - while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){ - i++;} - int distance3 = (receiveData3[0]<<8 )+ receiveData3[1]; - 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_cal)*cospi/cosp; - - //use moving average to find deltaX - int difference = abs(new_down_cal - distance3); - differenceAvgSum = differenceAvgSum - moving_ave[count2]; - moving_ave[count2] = difference; - differenceAvgSum = differenceAvgSum + difference; - 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-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[6] = 5; - intensity[6] = 7; - } else { - pulses[6] = 1; - intensity[6] = 0; - } - - if(count2 >4) { - count2 = 0; - } - } else { - - down_cal = distance3; - imu.get_angles(); - float pitch = imu.euler.pitch; - 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]); - - //pc.printf("about to send data\n"); - btData[0] = (pulses[0] << 5) | (intensity[0] << 2); - btData[1] = (pulses[1] << 4) | (intensity[1] << 1); - btData[2] = (pulses[2] << 3) | (intensity[2]); - btData[3] = (pulses[3] << 2) | (intensity[3] >> 1); - btData[4] = (intensity[3] << 7) | (pulses[4] << 1) | (intensity[4] >> 2); - btData[5] = (intensity[4] << 6) | (0x3); - btData[6] = (pulses[5] << 5) | (intensity[5] << 2); - btData[7] = (pulses[6] << 5) | (intensity[6] << 2); - btData[8] = '\0'; - for(int j=0;j<9;j++){ - if(bt.writeable()) - bt.putc(btData[j]); - //wait(0.001); - } - wait(0.05); + while (1) { + imu.get_angles(); + float pitch = imu.euler.pitch; + pc.printf("%f\n",pitch); +// pc.printf("about to send data\n"); +// for(int j=0;j<9;j++){ +// btData[j] = '\0'; +// if(bt.writeable()) +// bt.putc(btData[j]); +// //wait(0.001); +// } + wait(0.5); //pc.printf("finished sending data\n"); - //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9); } }