Robert Katzschmann / Mbed 2 deprecated bmbed_watch_imu

blue mbed code for the BNO055 imu from adafruit

Dependencies:   BNO055 MODSERIAL mbed

Fork of bmbed_lidar_belt by sensory_array

main.cpp@7:660e8ddb231e, 2015-10-14 (annotated)

Committer:
baraki
Date:
Wed Oct 14 20:33:38 2015 +0000
Revision:
7:660e8ddb231e
Parent:
3:a0ccaf565e8d
Child:
8:2ddeec5d8f84
now reads from IMU to calculate an allowable delta X for the downwards pointing lidar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
baraki 0:ce4f790399d9 1 #include "mbed.h"
baraki 0:ce4f790399d9 2 #include "MODSERIAL.h"
baraki 7:660e8ddb231e 3 #include <math.h>
baraki 7:660e8ddb231e 4 #include "BNO055.h"
baraki 0:ce4f790399d9 5
baraki 0:ce4f790399d9 6 #define PC_BAUD 9600
baraki 0:ce4f790399d9 7 #define BT_BAUD 9600
baraki 0:ce4f790399d9 8 #define TX_PIN p13
baraki 0:ce4f790399d9 9 #define RX_PIN p14
baraki 0:ce4f790399d9 10 #define SDA_PIN p9 //SDA pin on LPC1768
baraki 0:ce4f790399d9 11 #define SCL_PIN p10 //SCL pin on LPC1768
baraki 7:660e8ddb231e 12 #define IMU_SDA p28
baraki 7:660e8ddb231e 13 #define IMU_SCL p27
baraki 7:660e8ddb231e 14 #define PI 3.14159265
baraki 0:ce4f790399d9 15
baraki 7:660e8ddb231e 16 BNO055 imu(p28,p27);
baraki 0:ce4f790399d9 17 I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1
baraki 0:ce4f790399d9 18 MODSERIAL bt(TX_PIN, RX_PIN);
baraki 0:ce4f790399d9 19 MODSERIAL pc(USBTX,USBRX);
baraki 0:ce4f790399d9 20
baraki 7:660e8ddb231e 21 //for calculating allowable change in height for the down lidar
baraki 7:660e8ddb231e 22 float allowHeight = 120; //120 mm
baraki 7:660e8ddb231e 23 float downAngle = -7.0;
baraki 7:660e8ddb231e 24 //for calibrating IMU
baraki 7:660e8ddb231e 25 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};
baraki 7:660e8ddb231e 26
baraki 7:660e8ddb231e 27 //for encoder:
baraki 7:660e8ddb231e 28 //need to set pins to DigitalIn with internal pullup resistors
baraki 7:660e8ddb231e 29 //DigitalIn mySwitch(p21);
baraki 7:660e8ddb231e 30 //mySwitch.mode(PullUp);
baraki 7:660e8ddb231e 31
baraki 0:ce4f790399d9 32 bool newline_detected = false;
baraki 0:ce4f790399d9 33 bool newline_sent = false;
baraki 0:ce4f790399d9 34
baraki 7:660e8ddb231e 35 void setCal(){
baraki 7:660e8ddb231e 36 imu.write_calibration_data();
baraki 7:660e8ddb231e 37 }
baraki 7:660e8ddb231e 38
baraki 0:ce4f790399d9 39 // Called everytime a new character goes into
baraki 0:ce4f790399d9 40 // the RX buffer. Test that character for \n
baraki 0:ce4f790399d9 41 // Note, rxGetLastChar() gets the last char that
baraki 0:ce4f790399d9 42 // we received but it does NOT remove it from
baraki 0:ce4f790399d9 43 // the RX buffer.
baraki 0:ce4f790399d9 44 void rxCallback(MODSERIAL_IRQ_INFO *q)
baraki 0:ce4f790399d9 45 {
baraki 0:ce4f790399d9 46 MODSERIAL *serial = q->serial;
baraki 0:ce4f790399d9 47 if ( serial->rxGetLastChar() == '\n') {
baraki 0:ce4f790399d9 48 newline_detected = true;
baraki 0:ce4f790399d9 49 }
baraki 0:ce4f790399d9 50
baraki 0:ce4f790399d9 51 }
baraki 0:ce4f790399d9 52
baraki 0:ce4f790399d9 53 void txCallback(MODSERIAL_IRQ_INFO *q)
baraki 0:ce4f790399d9 54 {
baraki 0:ce4f790399d9 55 MODSERIAL *serial = q->serial;
baraki 0:ce4f790399d9 56 if ( serial->txGetLastChar() == '\0') {
baraki 0:ce4f790399d9 57 newline_sent = true;
baraki 0:ce4f790399d9 58 }
baraki 0:ce4f790399d9 59 }
baraki 0:ce4f790399d9 60
baraki 0:ce4f790399d9 61 int main()
baraki 0:ce4f790399d9 62 {
baraki 0:ce4f790399d9 63 pc.baud(PC_BAUD);
baraki 0:ce4f790399d9 64 bt.baud(BT_BAUD);
baraki 0:ce4f790399d9 65 pc.attach(&rxCallback, MODSERIAL::RxIrq);
baraki 0:ce4f790399d9 66 bt.attach(&txCallback, MODSERIAL::TxIrq);
baraki 0:ce4f790399d9 67
baraki 7:660e8ddb231e 68 //set up IMU
baraki 7:660e8ddb231e 69 imu.reset();
baraki 7:660e8ddb231e 70 imu.setmode(OPERATION_MODE_NDOF);
baraki 7:660e8ddb231e 71 setCal();
baraki 7:660e8ddb231e 72 imu.get_calib();
baraki 7:660e8ddb231e 73 while (imu.calib == 0)
baraki 7:660e8ddb231e 74 {
baraki 7:660e8ddb231e 75 imu.get_calib();
baraki 7:660e8ddb231e 76 }
baraki 7:660e8ddb231e 77
baraki 0:ce4f790399d9 78 sensor.frequency(100000);
baraki 0:ce4f790399d9 79
baraki 0:ce4f790399d9 80 char sendData[1] = {0x00};
baraki 0:ce4f790399d9 81
baraki 0:ce4f790399d9 82 int addresses[7];
baraki 0:ce4f790399d9 83 addresses[0] = 0x60; //0x60
baraki 0:ce4f790399d9 84 addresses[1] = 0x64; //0x64
baraki 0:ce4f790399d9 85 addresses[2] = 0x68; //middle
baraki 0:ce4f790399d9 86 addresses[3] = 0x6C;
baraki 0:ce4f790399d9 87 addresses[4] = 0x70;
baraki 0:ce4f790399d9 88 addresses[5] = 0x80; //up
baraki 0:ce4f790399d9 89 addresses[6] = 0x84; //down
baraki 0:ce4f790399d9 90
baraki 0:ce4f790399d9 91 uint8_t pulses[7] = {0};
baraki 0:ce4f790399d9 92 uint8_t intensity[7] = {0};
baraki 0:ce4f790399d9 93
baraki 0:ce4f790399d9 94 char btData[12] = {'a','b','c','d','e','f','g','\n','\0'};
baraki 0:ce4f790399d9 95
baraki 0:ce4f790399d9 96 //calibrate down sensor
baraki 0:ce4f790399d9 97 int down_cal = 0;
baraki 0:ce4f790399d9 98
baraki 0:ce4f790399d9 99 unsigned int i = 0;
baraki 0:ce4f790399d9 100 int count = 0; //for calibration
baraki 0:ce4f790399d9 101 int count2 = 0;//for averaging
baraki 0:ce4f790399d9 102 int differenceAvgSum = 0;
baraki 0:ce4f790399d9 103 int moving_ave[5] = {0};
baraki 0:ce4f790399d9 104 while (1) {
baraki 0:ce4f790399d9 105 for(int k=0; k<5; k++) {
baraki 0:ce4f790399d9 106 char receiveData[3] = {0};
baraki 0:ce4f790399d9 107 if(sensor.write(addresses[k], sendData, 1)){
baraki 1:5b1d88d69aa2 108 //pc.printf("writing to sensor %d failed\n", k);
baraki 0:ce4f790399d9 109 }
baraki 0:ce4f790399d9 110 //write ---> 0 on success, 1 on failure
baraki 0:ce4f790399d9 111 i = 0;
baraki 2:ec53792aef80 112 while(sensor.read(addresses[k], receiveData, 3) && i < 10) {
baraki 0:ce4f790399d9 113 i++;
baraki 1:5b1d88d69aa2 114 //pc.printf("reading from sensor %d failed\n",k);
baraki 1:5b1d88d69aa2 115 }
baraki 0:ce4f790399d9 116 //while(!twi_master_transfer(addresses[k], sendData, 1, TWI_ISSUE_STOP)){;}
baraki 0:ce4f790399d9 117 //while(!twi_master_transfer(addresses[k] + 1, receiveData, 3, TWI_ISSUE_STOP)){;}
baraki 0:ce4f790399d9 118 int distance = ((int)receiveData[0]<<8 )+ (int)receiveData[1];
baraki 0:ce4f790399d9 119 if(distance == 0){
baraki 0:ce4f790399d9 120 pulses[k] = 1;
baraki 0:ce4f790399d9 121 intensity[k] = 0;
baraki 0:ce4f790399d9 122 }
baraki 0:ce4f790399d9 123 if(distance > 0 && distance < 650) {
baraki 0:ce4f790399d9 124 pulses[k] = 5;
baraki 0:ce4f790399d9 125 intensity[k] = 7;
baraki 0:ce4f790399d9 126 } else if(distance >= 650 && distance < 900) {
baraki 0:ce4f790399d9 127 pulses[k] = 4;
baraki 0:ce4f790399d9 128 intensity[k] = 6;
baraki 0:ce4f790399d9 129 } else if(distance >= 900 && distance < 1350) {
baraki 0:ce4f790399d9 130 pulses[k] = 3;
baraki 0:ce4f790399d9 131 intensity[k] = 5;
baraki 0:ce4f790399d9 132 } else if(distance >= 1350 && distance < 1850) {
baraki 0:ce4f790399d9 133 pulses[k] = 2;
baraki 0:ce4f790399d9 134 intensity[k] = 2;
baraki 0:ce4f790399d9 135 } else if(distance >= 1800) {
baraki 0:ce4f790399d9 136 pulses[k] = 1;
baraki 0:ce4f790399d9 137 intensity[k] = 0;
baraki 0:ce4f790399d9 138 }
baraki 3:a0ccaf565e8d 139 //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",k,pulses[k],intensity[k]);
baraki 0:ce4f790399d9 140 }
baraki 0:ce4f790399d9 141
baraki 0:ce4f790399d9 142 //find UP distance
baraki 0:ce4f790399d9 143 char receiveData2[3] = {0};
baraki 0:ce4f790399d9 144 sensor.write(addresses[5], sendData, 1);
baraki 0:ce4f790399d9 145 i = 0;
baraki 2:ec53792aef80 146 while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
baraki 0:ce4f790399d9 147 i++;}
baraki 0:ce4f790399d9 148 int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
baraki 0:ce4f790399d9 149 if(distance2 >= 500 && distance2 < 1000) {
baraki 0:ce4f790399d9 150 pulses[5] = 5;
baraki 0:ce4f790399d9 151 intensity[5] = 7;
baraki 0:ce4f790399d9 152 } else {
baraki 0:ce4f790399d9 153 pulses[5] = 1;
baraki 0:ce4f790399d9 154 intensity[5] = 0;
baraki 0:ce4f790399d9 155 }
baraki 0:ce4f790399d9 156
baraki 0:ce4f790399d9 157 //find DOWN distance
baraki 0:ce4f790399d9 158 char receiveData3[3] = {0};
baraki 2:ec53792aef80 159 i = 0;
baraki 0:ce4f790399d9 160 sensor.write(addresses[6], sendData, 1);
baraki 2:ec53792aef80 161 while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){
baraki 0:ce4f790399d9 162 i++;}
baraki 0:ce4f790399d9 163 int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
baraki 7:660e8ddb231e 164 if(count > 200) { //calibration over
baraki 0:ce4f790399d9 165 int difference = abs(down_cal - distance3);
baraki 0:ce4f790399d9 166 differenceAvgSum = differenceAvgSum - moving_ave[count2];
baraki 0:ce4f790399d9 167 moving_ave[count2] = difference;
baraki 0:ce4f790399d9 168 differenceAvgSum = differenceAvgSum + difference;
baraki 0:ce4f790399d9 169 count2 = count2 + 1;
baraki 0:ce4f790399d9 170 int ave = (int)(differenceAvgSum/5);
baraki 7:660e8ddb231e 171
baraki 7:660e8ddb231e 172 //get allowableX from IMU
baraki 7:660e8ddb231e 173 imu.get_angles();
baraki 7:660e8ddb231e 174 float roll = imu.euler.roll;
baraki 7:660e8ddb231e 175 float pitch = imu.euler.pitch;
baraki 7:660e8ddb231e 176 float yaw = imu.euler.yaw;
baraki 7:660e8ddb231e 177 float cosp = cos((pitch-downAngle) * PI/180.0);
baraki 7:660e8ddb231e 178 float allowX = allowHeight/cosp;
baraki 7:660e8ddb231e 179 pc.printf("distance: %d\tallowableX: %f\r\n",ave,allowX);
baraki 7:660e8ddb231e 180
baraki 3:a0ccaf565e8d 181 //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
baraki 7:660e8ddb231e 182 if(ave >= allowX) {
baraki 0:ce4f790399d9 183 pulses[6] = 5;
baraki 0:ce4f790399d9 184 intensity[6] = 7;
baraki 0:ce4f790399d9 185 } else {
baraki 0:ce4f790399d9 186 pulses[6] = 1;
baraki 0:ce4f790399d9 187 intensity[6] = 0;
baraki 0:ce4f790399d9 188 }
baraki 0:ce4f790399d9 189
baraki 0:ce4f790399d9 190 if(count2 >4) {
baraki 0:ce4f790399d9 191 count2 = 0;
baraki 0:ce4f790399d9 192 }
baraki 0:ce4f790399d9 193 } else {
baraki 0:ce4f790399d9 194 down_cal = distance3;
baraki 0:ce4f790399d9 195 count = count+1;
baraki 0:ce4f790399d9 196 }
baraki 3:a0ccaf565e8d 197 //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",6,pulses[6],intensity[6]);
baraki 0:ce4f790399d9 198
baraki 1:5b1d88d69aa2 199 //pc.printf("about to send data\n");
baraki 0:ce4f790399d9 200 btData[0] = (pulses[0] << 5) | (intensity[0] << 2);
baraki 0:ce4f790399d9 201 btData[1] = (pulses[1] << 4) | (intensity[1] << 1);
baraki 0:ce4f790399d9 202 btData[2] = (pulses[2] << 3) | (intensity[2]);
baraki 0:ce4f790399d9 203 btData[3] = (pulses[3] << 2) | (intensity[3] >> 1);
baraki 0:ce4f790399d9 204 btData[4] = (intensity[3] << 7) | (pulses[4] << 1) | (intensity[4] >> 2);
baraki 0:ce4f790399d9 205 btData[5] = (intensity[4] << 6) | (0x3);
baraki 0:ce4f790399d9 206 btData[6] = (pulses[5] << 5) | (intensity[5] << 2);
baraki 0:ce4f790399d9 207 btData[7] = (pulses[6] << 5) | (intensity[6] << 2);
baraki 0:ce4f790399d9 208 btData[8] = '\0';
baraki 0:ce4f790399d9 209 for(int j=0;j<9;j++){
baraki 1:5b1d88d69aa2 210 if(bt.writeable())
baraki 1:5b1d88d69aa2 211 bt.putc(btData[j]);
baraki 1:5b1d88d69aa2 212 //wait(0.001);
baraki 0:ce4f790399d9 213 }
baraki 3:a0ccaf565e8d 214 wait(0.05);
baraki 1:5b1d88d69aa2 215 //pc.printf("finished sending data\n");
baraki 0:ce4f790399d9 216 //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9);
baraki 0:ce4f790399d9 217 }
baraki 0:ce4f790399d9 218 }