blue mbed code for the BNO055 imu from adafruit
Dependencies: BNO055 MODSERIAL mbed
Fork of bmbed_lidar_belt by
Diff: main.cpp
- Revision:
- 7:660e8ddb231e
- Parent:
- 3:a0ccaf565e8d
- Child:
- 8:2ddeec5d8f84
--- a/main.cpp Fri Sep 25 15:14:10 2015 +0000 +++ b/main.cpp Wed Oct 14 20:33:38 2015 +0000 @@ -1,5 +1,7 @@ #include "mbed.h" #include "MODSERIAL.h" +#include <math.h> +#include "BNO055.h" #define PC_BAUD 9600 #define BT_BAUD 9600 @@ -7,14 +9,33 @@ #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 = -7.0; +//for calibrating IMU +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 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 @@ -44,6 +65,16 @@ 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}; @@ -130,15 +161,25 @@ while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){ i++;} int distance3 = (receiveData3[0]<<8 )+ receiveData3[1]; - if(count > 200) { + if(count > 200) { //calibration over int difference = abs(down_cal - distance3); differenceAvgSum = differenceAvgSum - moving_ave[count2]; moving_ave[count2] = difference; differenceAvgSum = differenceAvgSum + difference; count2 = count2 + 1; int ave = (int)(differenceAvgSum/5); + + //get allowableX from IMU + imu.get_angles(); + float roll = imu.euler.roll; + float pitch = imu.euler.pitch; + float yaw = imu.euler.yaw; + float cosp = cos((pitch-downAngle) * PI/180.0); + float allowX = allowHeight/cosp; + pc.printf("distance: %d\tallowableX: %f\r\n",ave,allowX); + //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3); - if(ave >= 160) { + if(ave >= allowX) { pulses[6] = 5; intensity[6] = 7; } else {