blue mbed code for the BNO055 imu from adafruit
Dependencies: BNO055 MODSERIAL mbed
Fork of bmbed_lidar_belt by
main.cpp
- Committer:
- baraki
- Date:
- 2015-10-16
- Revision:
- 9:59d23ab8d73b
- Parent:
- 8:2ddeec5d8f84
- Child:
- 11:c972bf9c24de
File content as of revision 9:59d23ab8d73b:
#include "mbed.h" #include "MODSERIAL.h" #include <math.h> #include "BNO055.h" #define PC_BAUD 9600 #define BT_BAUD 9600 #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 IMU_SDA p28 #define IMU_SCL p27 #define PI 3.14159265 BNO055 imu(p28,p27); I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1 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}; //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); bool newline_detected = false; bool newline_sent = false; void setCal(){ imu.write_calibration_data(); } // Called everytime a new character goes into // the RX buffer. Test that character for \n // Note, rxGetLastChar() gets the last char that // we received but it does NOT remove it from // the RX buffer. void rxCallback(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; if ( serial->rxGetLastChar() == '\n') { newline_detected = true; } } void txCallback(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; if ( serial->txGetLastChar() == '\0') { newline_sent = true; } } int main() { pc.baud(PC_BAUD); bt.baud(BT_BAUD); pc.attach(&rxCallback, MODSERIAL::RxIrq); bt.attach(&txCallback, MODSERIAL::TxIrq); //set up IMU imu.reset(); imu.setmode(OPERATION_MODE_NDOF); setCal(); imu.get_calib(); while (imu.calib == 0) { 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; float cospi = 0; //"cosine of initial pitch" unsigned int i = 0; int count = 0; //for calibration int count2 = 0;//for averaging int differenceAvgSum = 0; int moving_ave[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]; if(distance == 0){ pulses[k] = 1; intensity[k] = 0; } if(distance > 0 && distance < 650) { pulses[k] = 5; intensity[k] = 7; } else if(distance >= 650 && distance < 900) { pulses[k] = 4; intensity[k] = 6; } else if(distance >= 900 && distance < 1350) { pulses[k] = 3; intensity[k] = 5; } else if(distance >= 1350 && distance < 1850) { pulses[k] = 2; intensity[k] = 2; } else if(distance >= 1800) { 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 >= 500 && distance2 < 1000) { pulses[5] = 5; intensity[5] = 7; } else { pulses[5] = 1; intensity[5] = 0; } //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 > 100) { //calibration over //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 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-downAngle,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-downAngle) * 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); //pc.printf("finished sending data\n"); //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9); } }