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@5:d4ed744beea2, 2015-10-03 (annotated)

Committer:
rkk
Date:
Sat Oct 03 23:10:51 2015 +0000
Revision:
5:d4ed744beea2
Parent:
4:c53761262e3f
Child:
6:9ae0e867efed
turned off up and down!

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 0:ce4f790399d9 3
rkk 5:d4ed744beea2 4 //Initial LONG Range Settings
rkk 5:d4ed744beea2 5 #define RANGE_01 750
rkk 5:d4ed744beea2 6 #define RANGE_02 1100
rkk 5:d4ed744beea2 7 #define RANGE_03 1700
rkk 5:d4ed744beea2 8 #define RANGE_04 2750
rkk 5:d4ed744beea2 9 #define UP_MIN 600
rkk 5:d4ed744beea2 10 #define UP_MAX 1300
rkk 5:d4ed744beea2 11 #define DOWN_DIFF 500
rkk 5:d4ed744beea2 12
rkk 5:d4ed744beea2 13 // Mid range settings
rkk 5:d4ed744beea2 14 //#define RANGE_01 450
rkk 5:d4ed744beea2 15 //#define RANGE_02 750
rkk 5:d4ed744beea2 16 //#define RANGE_03 1150
rkk 5:d4ed744beea2 17 //#define RANGE_04 1550
rkk 5:d4ed744beea2 18 //#define UP_MIN 600
rkk 5:d4ed744beea2 19 //#define UP_MAX 1300
rkk 5:d4ed744beea2 20 //#define DOWN_DIFF 260
rkk 5:d4ed744beea2 21
rkk 5:d4ed744beea2 22 // Short range settings
rkk 5:d4ed744beea2 23 //#define RANGE_01 400
rkk 5:d4ed744beea2 24 //#define RANGE_02 650
rkk 5:d4ed744beea2 25 //#define RANGE_03 1000
rkk 5:d4ed744beea2 26 //#define RANGE_04 1350
rkk 5:d4ed744beea2 27 //#define UP_MIN 600
rkk 5:d4ed744beea2 28 //#define UP_MAX 1300
rkk 5:d4ed744beea2 29 //#define DOWN_DIFF 260
rkk 5:d4ed744beea2 30
rkk 5:d4ed744beea2 31
baraki 0:ce4f790399d9 32 #define PC_BAUD 9600
rkk 4:c53761262e3f 33 #define BT_BAUD 115200
baraki 0:ce4f790399d9 34 #define TX_PIN p13
baraki 0:ce4f790399d9 35 #define RX_PIN p14
baraki 0:ce4f790399d9 36 #define SDA_PIN p9 //SDA pin on LPC1768
baraki 0:ce4f790399d9 37 #define SCL_PIN p10 //SCL pin on LPC1768
baraki 0:ce4f790399d9 38
rkk 5:d4ed744beea2 39
rkk 5:d4ed744beea2 40
baraki 0:ce4f790399d9 41 I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1
baraki 0:ce4f790399d9 42 MODSERIAL bt(TX_PIN, RX_PIN);
baraki 0:ce4f790399d9 43 MODSERIAL pc(USBTX,USBRX);
baraki 0:ce4f790399d9 44
baraki 0:ce4f790399d9 45 bool newline_detected = false;
baraki 0:ce4f790399d9 46 bool newline_sent = false;
baraki 0:ce4f790399d9 47
baraki 0:ce4f790399d9 48 // Called everytime a new character goes into
baraki 0:ce4f790399d9 49 // the RX buffer. Test that character for \n
baraki 0:ce4f790399d9 50 // Note, rxGetLastChar() gets the last char that
baraki 0:ce4f790399d9 51 // we received but it does NOT remove it from
baraki 0:ce4f790399d9 52 // the RX buffer.
baraki 0:ce4f790399d9 53 void rxCallback(MODSERIAL_IRQ_INFO *q)
baraki 0:ce4f790399d9 54 {
baraki 0:ce4f790399d9 55 MODSERIAL *serial = q->serial;
baraki 0:ce4f790399d9 56 if ( serial->rxGetLastChar() == '\n') {
baraki 0:ce4f790399d9 57 newline_detected = true;
baraki 0:ce4f790399d9 58 }
baraki 0:ce4f790399d9 59
baraki 0:ce4f790399d9 60 }
baraki 0:ce4f790399d9 61
baraki 0:ce4f790399d9 62 void txCallback(MODSERIAL_IRQ_INFO *q)
baraki 0:ce4f790399d9 63 {
baraki 0:ce4f790399d9 64 MODSERIAL *serial = q->serial;
baraki 0:ce4f790399d9 65 if ( serial->txGetLastChar() == '\0') {
baraki 0:ce4f790399d9 66 newline_sent = true;
baraki 0:ce4f790399d9 67 }
baraki 0:ce4f790399d9 68 }
baraki 0:ce4f790399d9 69
baraki 0:ce4f790399d9 70 int main()
baraki 0:ce4f790399d9 71 {
baraki 0:ce4f790399d9 72 pc.baud(PC_BAUD);
baraki 0:ce4f790399d9 73 bt.baud(BT_BAUD);
baraki 0:ce4f790399d9 74 pc.attach(&rxCallback, MODSERIAL::RxIrq);
baraki 0:ce4f790399d9 75 bt.attach(&txCallback, MODSERIAL::TxIrq);
baraki 0:ce4f790399d9 76
baraki 0:ce4f790399d9 77 sensor.frequency(100000);
baraki 0:ce4f790399d9 78
baraki 0:ce4f790399d9 79 char sendData[1] = {0x00};
baraki 0:ce4f790399d9 80
baraki 0:ce4f790399d9 81 int addresses[7];
baraki 0:ce4f790399d9 82 addresses[0] = 0x60; //0x60
baraki 0:ce4f790399d9 83 addresses[1] = 0x64; //0x64
baraki 0:ce4f790399d9 84 addresses[2] = 0x68; //middle
baraki 0:ce4f790399d9 85 addresses[3] = 0x6C;
baraki 0:ce4f790399d9 86 addresses[4] = 0x70;
baraki 0:ce4f790399d9 87 addresses[5] = 0x80; //up
baraki 0:ce4f790399d9 88 addresses[6] = 0x84; //down
baraki 0:ce4f790399d9 89
baraki 0:ce4f790399d9 90 uint8_t pulses[7] = {0};
baraki 0:ce4f790399d9 91 uint8_t intensity[7] = {0};
baraki 0:ce4f790399d9 92
baraki 0:ce4f790399d9 93 char btData[12] = {'a','b','c','d','e','f','g','\n','\0'};
baraki 0:ce4f790399d9 94
baraki 0:ce4f790399d9 95 //calibrate down sensor
baraki 0:ce4f790399d9 96 int down_cal = 0;
baraki 0:ce4f790399d9 97
baraki 0:ce4f790399d9 98 unsigned int i = 0;
baraki 0:ce4f790399d9 99 int count = 0; //for calibration
baraki 0:ce4f790399d9 100 int count2 = 0;//for averaging
baraki 0:ce4f790399d9 101 int differenceAvgSum = 0;
baraki 0:ce4f790399d9 102 int moving_ave[5] = {0};
baraki 0:ce4f790399d9 103 while (1) {
baraki 0:ce4f790399d9 104 for(int k=0; k<5; k++) {
baraki 0:ce4f790399d9 105 char receiveData[3] = {0};
baraki 0:ce4f790399d9 106 if(sensor.write(addresses[k], sendData, 1)){
baraki 1:5b1d88d69aa2 107 //pc.printf("writing to sensor %d failed\n", k);
baraki 0:ce4f790399d9 108 }
baraki 0:ce4f790399d9 109 //write ---> 0 on success, 1 on failure
baraki 0:ce4f790399d9 110 i = 0;
baraki 2:ec53792aef80 111 while(sensor.read(addresses[k], receiveData, 3) && i < 10) {
baraki 0:ce4f790399d9 112 i++;
baraki 1:5b1d88d69aa2 113 //pc.printf("reading from sensor %d failed\n",k);
baraki 1:5b1d88d69aa2 114 }
baraki 0:ce4f790399d9 115 //while(!twi_master_transfer(addresses[k], sendData, 1, TWI_ISSUE_STOP)){;}
baraki 0:ce4f790399d9 116 //while(!twi_master_transfer(addresses[k] + 1, receiveData, 3, TWI_ISSUE_STOP)){;}
baraki 0:ce4f790399d9 117 int distance = ((int)receiveData[0]<<8 )+ (int)receiveData[1];
baraki 0:ce4f790399d9 118 if(distance == 0){
baraki 0:ce4f790399d9 119 pulses[k] = 1;
baraki 0:ce4f790399d9 120 intensity[k] = 0;
baraki 0:ce4f790399d9 121 }
rkk 5:d4ed744beea2 122 if(distance > 0 && distance < RANGE_01) {
baraki 0:ce4f790399d9 123 pulses[k] = 5;
baraki 0:ce4f790399d9 124 intensity[k] = 7;
rkk 5:d4ed744beea2 125 } else if(distance >= RANGE_01 && distance < RANGE_02) {
baraki 0:ce4f790399d9 126 pulses[k] = 4;
baraki 0:ce4f790399d9 127 intensity[k] = 6;
rkk 5:d4ed744beea2 128 } else if(distance >= RANGE_02 && distance < RANGE_03) {
baraki 0:ce4f790399d9 129 pulses[k] = 3;
baraki 0:ce4f790399d9 130 intensity[k] = 5;
rkk 5:d4ed744beea2 131 } else if(distance >= RANGE_03 && distance < RANGE_04) {
baraki 0:ce4f790399d9 132 pulses[k] = 2;
baraki 0:ce4f790399d9 133 intensity[k] = 2;
rkk 5:d4ed744beea2 134 } else if(distance >= RANGE_04) {
baraki 0:ce4f790399d9 135 pulses[k] = 1;
baraki 0:ce4f790399d9 136 intensity[k] = 0;
baraki 0:ce4f790399d9 137 }
baraki 3:a0ccaf565e8d 138 //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",k,pulses[k],intensity[k]);
baraki 0:ce4f790399d9 139 }
baraki 0:ce4f790399d9 140
baraki 0:ce4f790399d9 141 //find UP distance
baraki 0:ce4f790399d9 142 char receiveData2[3] = {0};
baraki 0:ce4f790399d9 143 sensor.write(addresses[5], sendData, 1);
baraki 0:ce4f790399d9 144 i = 0;
baraki 2:ec53792aef80 145 while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
baraki 0:ce4f790399d9 146 i++;}
baraki 0:ce4f790399d9 147 int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
rkk 5:d4ed744beea2 148 if(distance2 >= UP_MIN && distance2 < UP_MAX) {
rkk 5:d4ed744beea2 149 pulses[5] = 1; ///5
rkk 5:d4ed744beea2 150 intensity[5] = 0; ///7
baraki 0:ce4f790399d9 151 } else {
baraki 0:ce4f790399d9 152 pulses[5] = 1;
baraki 0:ce4f790399d9 153 intensity[5] = 0;
baraki 0:ce4f790399d9 154 }
baraki 0:ce4f790399d9 155
baraki 0:ce4f790399d9 156 //find DOWN distance
baraki 0:ce4f790399d9 157 char receiveData3[3] = {0};
baraki 2:ec53792aef80 158 i = 0;
baraki 0:ce4f790399d9 159 sensor.write(addresses[6], sendData, 1);
baraki 2:ec53792aef80 160 while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){
baraki 0:ce4f790399d9 161 i++;}
baraki 0:ce4f790399d9 162 int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
baraki 3:a0ccaf565e8d 163 if(count > 200) {
baraki 0:ce4f790399d9 164 int difference = abs(down_cal - distance3);
baraki 0:ce4f790399d9 165 differenceAvgSum = differenceAvgSum - moving_ave[count2];
baraki 0:ce4f790399d9 166 moving_ave[count2] = difference;
baraki 0:ce4f790399d9 167 differenceAvgSum = differenceAvgSum + difference;
baraki 0:ce4f790399d9 168 count2 = count2 + 1;
baraki 0:ce4f790399d9 169 int ave = (int)(differenceAvgSum/5);
baraki 3:a0ccaf565e8d 170 //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
rkk 5:d4ed744beea2 171 if(ave >= DOWN_DIFF) {
rkk 5:d4ed744beea2 172 pulses[6] = 1; //turned OFFFFFF
rkk 5:d4ed744beea2 173 intensity[6] = 0;
baraki 0:ce4f790399d9 174 } else {
rkk 5:d4ed744beea2 175 pulses[6] = 1;
baraki 0:ce4f790399d9 176 intensity[6] = 0;
baraki 0:ce4f790399d9 177 }
baraki 0:ce4f790399d9 178
baraki 0:ce4f790399d9 179 if(count2 >4) {
baraki 0:ce4f790399d9 180 count2 = 0;
baraki 0:ce4f790399d9 181 }
baraki 0:ce4f790399d9 182 } else {
baraki 0:ce4f790399d9 183 down_cal = distance3;
baraki 0:ce4f790399d9 184 count = count+1;
baraki 0:ce4f790399d9 185 }
baraki 3:a0ccaf565e8d 186 //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",6,pulses[6],intensity[6]);
baraki 0:ce4f790399d9 187
baraki 1:5b1d88d69aa2 188 //pc.printf("about to send data\n");
baraki 0:ce4f790399d9 189 btData[0] = (pulses[0] << 5) | (intensity[0] << 2);
baraki 0:ce4f790399d9 190 btData[1] = (pulses[1] << 4) | (intensity[1] << 1);
baraki 0:ce4f790399d9 191 btData[2] = (pulses[2] << 3) | (intensity[2]);
baraki 0:ce4f790399d9 192 btData[3] = (pulses[3] << 2) | (intensity[3] >> 1);
baraki 0:ce4f790399d9 193 btData[4] = (intensity[3] << 7) | (pulses[4] << 1) | (intensity[4] >> 2);
baraki 0:ce4f790399d9 194 btData[5] = (intensity[4] << 6) | (0x3);
baraki 0:ce4f790399d9 195 btData[6] = (pulses[5] << 5) | (intensity[5] << 2);
baraki 0:ce4f790399d9 196 btData[7] = (pulses[6] << 5) | (intensity[6] << 2);
baraki 0:ce4f790399d9 197 btData[8] = '\0';
baraki 0:ce4f790399d9 198 for(int j=0;j<9;j++){
baraki 1:5b1d88d69aa2 199 if(bt.writeable())
baraki 1:5b1d88d69aa2 200 bt.putc(btData[j]);
baraki 1:5b1d88d69aa2 201 //wait(0.001);
baraki 0:ce4f790399d9 202 }
baraki 3:a0ccaf565e8d 203 wait(0.05);
baraki 1:5b1d88d69aa2 204 //pc.printf("finished sending data\n");
baraki 0:ce4f790399d9 205 //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9);
baraki 0:ce4f790399d9 206 }
baraki 0:ce4f790399d9 207 }