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@13:b22fd9c4fbb4, 2015-10-16 (annotated)

Committer:
rkk
Date:
Fri Oct 16 20:10:10 2015 +0000
Revision:
13:b22fd9c4fbb4
Parent:
12:6deb3b41c9e3
Child:
14:8620dceb3ba2
adjusted hash defines

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