sensory_array / Mbed 2 deprecated bmbed_lidar_belt_braille

Dependencies:   BNO055 MODSERIAL mbed

main.cpp@2:7da645fba3eb, 2015-11-12 (annotated)

Committer:
baraki
Date:
Thu Nov 12 22:05:17 2015 +0000
Revision:
2:7da645fba3eb
Parent:
1:86b2752371c2 
now implements the new display mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
baraki 0:11e8c8e09a07 1 #include "mbed.h"
baraki 0:11e8c8e09a07 2 #include "MODSERIAL.h"
baraki 0:11e8c8e09a07 3 #include <math.h>
baraki 0:11e8c8e09a07 4 #include "BNO055.h"
baraki 0:11e8c8e09a07 5 //for calculating allowable change in height for the down lidar
baraki 0:11e8c8e09a07 6 #define DOWN_ANGLE -3.8 //#1
baraki 0:11e8c8e09a07 7 //#define DOWN_ANGLE -8.3 //#2
baraki 0:11e8c8e09a07 8 //#define DOWN_ANGLE -9.0 //#3
baraki 0:11e8c8e09a07 9 #define ALLOW_HEIGHT 120
baraki 0:11e8c8e09a07 10
baraki 0:11e8c8e09a07 11 //Up Delta
baraki 0:11e8c8e09a07 12 #define UP_MIN 500
baraki 0:11e8c8e09a07 13 #define UP_MAX 1100
baraki 0:11e8c8e09a07 14
baraki 0:11e8c8e09a07 15 //for scaling braille output
baraki 0:11e8c8e09a07 16 float MAX_MAX_braille = 3000.0; //maximum max distance braille can see
baraki 0:11e8c8e09a07 17 float MIN_MAX_braille = 1000.0; //minimum max distance braile can see
baraki 0:11e8c8e09a07 18 float MIN_braille = 210.0; //the minimum that braille can see is 200 (limited by LIDAR)
baraki 0:11e8c8e09a07 19 #define BRAILLE_STEPS 7 //only 7 right now... ideally 8 but I messed up braille code a bit
baraki 0:11e8c8e09a07 20 bool brailleMode = 1; //1 if brailleMode; 0 if vibrator mode
baraki 0:11e8c8e09a07 21 char brailleChar[12] = {0};
baraki 0:11e8c8e09a07 22
baraki 0:11e8c8e09a07 23 float PMIN_EMIN_A=300.0; //min side range
baraki 0:11e8c8e09a07 24 float PMAX_EMIN_A=700.0; //min side range
baraki 0:11e8c8e09a07 25 float PMIN_EMAX_A=900.0; //max side range
baraki 0:11e8c8e09a07 26 float PMAX_EMAX_A=1600.0; //max side range
baraki 0:11e8c8e09a07 27
baraki 0:11e8c8e09a07 28 float PMIN_EMIN_B=400.0; //min front range
baraki 0:11e8c8e09a07 29 float PMAX_EMIN_B=1000.0; //min front range
baraki 0:11e8c8e09a07 30 float PMIN_EMAX_B=1200.0; //max front range
baraki 0:11e8c8e09a07 31 float PMAX_EMAX_B=2750.0; //max front range
baraki 0:11e8c8e09a07 32
baraki 0:11e8c8e09a07 33 //Initial LONG Range Settings
baraki 0:11e8c8e09a07 34 //#define RANGE_01 750
baraki 0:11e8c8e09a07 35 //#define RANGE_02 1100
baraki 0:11e8c8e09a07 36 //#define RANGE_03 1700
baraki 0:11e8c8e09a07 37 //#define RANGE_04 2750
baraki 0:11e8c8e09a07 38 //#define UP_MIN 600
baraki 0:11e8c8e09a07 39 //#define UP_MAX 1300
baraki 0:11e8c8e09a07 40
baraki 0:11e8c8e09a07 41 // Mid range settings
baraki 0:11e8c8e09a07 42 #define RANGE_01 450
baraki 0:11e8c8e09a07 43 #define RANGE_02 750
baraki 0:11e8c8e09a07 44 #define RANGE_03 1150
baraki 0:11e8c8e09a07 45 #define RANGE_04 1550
baraki 0:11e8c8e09a07 46
baraki 0:11e8c8e09a07 47 // Short range settings
baraki 0:11e8c8e09a07 48 //#define RANGE_01 400
baraki 0:11e8c8e09a07 49 //#define RANGE_02 650
baraki 0:11e8c8e09a07 50 //#define RANGE_03 1000
baraki 0:11e8c8e09a07 51 //#define RANGE_04 1350
baraki 0:11e8c8e09a07 52
baraki 0:11e8c8e09a07 53
baraki 0:11e8c8e09a07 54 #define PC_BAUD 9600
baraki 0:11e8c8e09a07 55 #define BT_BAUD 115200
baraki 0:11e8c8e09a07 56 #define TX_PIN p13
baraki 0:11e8c8e09a07 57 #define RX_PIN p14
baraki 0:11e8c8e09a07 58 #define SDA_PIN p9 //SDA pin on LPC1768
baraki 0:11e8c8e09a07 59 #define SCL_PIN p10 //SCL pin on LPC1768
baraki 0:11e8c8e09a07 60 #define IMU_SDA p28
baraki 0:11e8c8e09a07 61 #define IMU_SCL p27
baraki 0:11e8c8e09a07 62 #define PI 3.14159265
baraki 0:11e8c8e09a07 63
baraki 0:11e8c8e09a07 64 float angle[5] = {PI, 3*PI/4, PI/2, PI/4, 0}; //relates lidar number to its angle in radians
baraki 0:11e8c8e09a07 65
baraki 0:11e8c8e09a07 66 BNO055 imu(p28,p27);
baraki 0:11e8c8e09a07 67
baraki 0:11e8c8e09a07 68 I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1
baraki 0:11e8c8e09a07 69 MODSERIAL bt(TX_PIN, RX_PIN);
baraki 0:11e8c8e09a07 70 MODSERIAL pc(USBTX,USBRX);
baraki 0:11e8c8e09a07 71 AnalogIn potentiometer(p20);
baraki 0:11e8c8e09a07 72
baraki 0:11e8c8e09a07 73
baraki 0:11e8c8e09a07 74 //for calibrating IMU
baraki 0:11e8c8e09a07 75 //for IMU1:
baraki 0:11e8c8e09a07 76 //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 0:11e8c8e09a07 77 //for IMU2:
baraki 0:11e8c8e09a07 78 //char cal_vals[22] = {239, 255, 27, 0, 5, 0, 76, 255, 230, 0, 115, 255, 255, 255, 254, 255, 1, 0, 232, 3, 106, 3}; //#2
baraki 0:11e8c8e09a07 79 char cal_vals[22] = {255, 255, 2, 0, 18, 0, 58, 255, 236, 0, 156, 255, 1, 0, 255, 255, 255, 255, 232, 3, 43, 3}; //#1
baraki 0:11e8c8e09a07 80 //char cal_vals[22] = {237, 255, 18, 0, 24, 0, 144, 255, 232, 0, 54, 255, 0, 0, 254, 255, 0, 0, 232, 3, 87, 3}; //#3
baraki 0:11e8c8e09a07 81
baraki 0:11e8c8e09a07 82 //for encoder:
baraki 0:11e8c8e09a07 83 //need to set pins to DigitalIn with internal pullup resistors
baraki 0:11e8c8e09a07 84 //DigitalIn mySwitch(p21);
baraki 0:11e8c8e09a07 85 //mySwitch.mode(PullUp);
baraki 0:11e8c8e09a07 86
baraki 0:11e8c8e09a07 87 bool newline_detected = false;
baraki 0:11e8c8e09a07 88 bool newline_sent = false;
baraki 0:11e8c8e09a07 89
baraki 0:11e8c8e09a07 90 // lowest at 0 and 1; highest at 0.5
baraki 0:11e8c8e09a07 91 float potValue(){
baraki 0:11e8c8e09a07 92 float pot = potentiometer.read();
baraki 0:11e8c8e09a07 93 //pc.printf("pot: %f ",pot);
baraki 0:11e8c8e09a07 94 //float pot = ((float)((char)pc.getc()) - '0')/18.0;
baraki 0:11e8c8e09a07 95 if(pot <= 0.5)
baraki 0:11e8c8e09a07 96 return 2*pot;
baraki 0:11e8c8e09a07 97 else
baraki 0:11e8c8e09a07 98 return 2*(1-pot); //if pot = 1, return 0. if pot = 0.5, return 1
baraki 0:11e8c8e09a07 99 }
baraki 0:11e8c8e09a07 100
baraki 0:11e8c8e09a07 101 void angleToRadius(float* radius,float angle){
baraki 0:11e8c8e09a07 102 //a is horizontal semi-axis
baraki 0:11e8c8e09a07 103 //b is vertical semi-axis
baraki 0:11e8c8e09a07 104 //scale between MIN_A, MAX_A, MIN_B, MAX_B
baraki 0:11e8c8e09a07 105 //using pot value
baraki 0:11e8c8e09a07 106 float pot = potValue();
baraki 0:11e8c8e09a07 107 //define new min_a scaled between PMIN_EMIN_A and PMAX_EMIN_A
baraki 0:11e8c8e09a07 108 float min_a = PMIN_EMIN_A + (PMAX_EMIN_A - PMIN_EMIN_A)*pot;
baraki 0:11e8c8e09a07 109 //define new max_a scaled between PMIN_EMAX_A and PMAX_EMAX_A
baraki 0:11e8c8e09a07 110 float max_a = PMIN_EMAX_A + (PMAX_EMAX_A - PMIN_EMAX_A)*pot;
baraki 0:11e8c8e09a07 111 //define new min_b between PMIN_EMIN_B and PMAX_EMIN_B
baraki 0:11e8c8e09a07 112 float min_b = PMIN_EMIN_B + (PMAX_EMIN_B - PMIN_EMIN_B)*pot;
baraki 0:11e8c8e09a07 113 //define new max_b scaled between PMIN_EMAX_B and PMAX_EMAX_B
baraki 0:11e8c8e09a07 114 float max_b = PMIN_EMAX_B + (PMAX_EMAX_B - PMIN_EMAX_B)*pot;
baraki 0:11e8c8e09a07 115
baraki 0:11e8c8e09a07 116 float scale[4] = {0, 0.25, 0.6,1.0};
baraki 0:11e8c8e09a07 117 //0 is min range
baraki 0:11e8c8e09a07 118 //higher is higher range
baraki 0:11e8c8e09a07 119 for(int i=0;i<4;i++){
baraki 0:11e8c8e09a07 120 //scale a and be to be somewhere between the min and max based on i
baraki 0:11e8c8e09a07 121 // i = 0 -- min
baraki 0:11e8c8e09a07 122 // i = 5 -- max
baraki 0:11e8c8e09a07 123 float a = min_a + (max_a - min_a)*scale[i];
baraki 0:11e8c8e09a07 124 float b = min_b + (max_b - min_b)*scale[i];
baraki 0:11e8c8e09a07 125 radius[i] = a*b/sqrt(pow(a,2)*pow(sin(angle),2) + pow(b,2)*pow(cos(angle),2));
baraki 0:11e8c8e09a07 126 }
baraki 0:11e8c8e09a07 127 }
baraki 0:11e8c8e09a07 128
baraki 0:11e8c8e09a07 129 void setCal(){
baraki 0:11e8c8e09a07 130 imu.write_calibration_data();
baraki 0:11e8c8e09a07 131 }
baraki 0:11e8c8e09a07 132
baraki 0:11e8c8e09a07 133 // Called everytime a new character goes into
baraki 0:11e8c8e09a07 134 // the RX buffer. Test that character for \n
baraki 0:11e8c8e09a07 135 // Note, rxGetLastChar() gets the last char that
baraki 0:11e8c8e09a07 136 // we received but it does NOT remove it from
baraki 0:11e8c8e09a07 137 // the RX buffer.
baraki 0:11e8c8e09a07 138 void rxCallback(MODSERIAL_IRQ_INFO *q)
baraki 0:11e8c8e09a07 139 {
baraki 0:11e8c8e09a07 140 MODSERIAL *serial = q->serial;
baraki 0:11e8c8e09a07 141 if ( serial->rxGetLastChar() == '\n') {
baraki 0:11e8c8e09a07 142 newline_detected = true;
baraki 0:11e8c8e09a07 143 }
baraki 0:11e8c8e09a07 144
baraki 0:11e8c8e09a07 145 }
baraki 0:11e8c8e09a07 146
baraki 0:11e8c8e09a07 147 void txCallback(MODSERIAL_IRQ_INFO *q)
baraki 0:11e8c8e09a07 148 {
baraki 0:11e8c8e09a07 149 MODSERIAL *serial = q->serial;
baraki 0:11e8c8e09a07 150 if ( serial->txGetLastChar() == '\0') {
baraki 0:11e8c8e09a07 151 newline_sent = true;
baraki 0:11e8c8e09a07 152 }
baraki 0:11e8c8e09a07 153 }
baraki 0:11e8c8e09a07 154
baraki 0:11e8c8e09a07 155 void dist_to_str(float dist, char* braille){
baraki 0:11e8c8e09a07 156 // the low numbers are farther away
baraki 0:11e8c8e09a07 157 //braille must be at least 9 chars long
baraki 0:11e8c8e09a07 158 braille[0] = '1';
baraki 0:11e8c8e09a07 159 braille[8] = '\0';
baraki 0:11e8c8e09a07 160 float max_braille = MIN_MAX_braille + (MAX_MAX_braille - MIN_MAX_braille)*potValue();
baraki 0:11e8c8e09a07 161 float step_size = (max_braille - MIN_braille)/BRAILLE_STEPS;
baraki 0:11e8c8e09a07 162 for(int i = 1;i<8;i++){
baraki 0:11e8c8e09a07 163 if(dist > (MIN_braille + step_size*(BRAILLE_STEPS - i))) //if dist is greater than a certain step size, that step is clear
baraki 0:11e8c8e09a07 164 braille[i] = '0';
baraki 0:11e8c8e09a07 165 else
baraki 0:11e8c8e09a07 166 braille[i] = '1';
baraki 0:11e8c8e09a07 167 }
baraki 0:11e8c8e09a07 168 }
baraki 0:11e8c8e09a07 169
baraki 0:11e8c8e09a07 170 char str_to_bin(char *str){
baraki 1:86b2752371c2 171 char brailleCharacter = 0;
baraki 0:11e8c8e09a07 172 for(int i=0;i<8;i++){
baraki 0:11e8c8e09a07 173 int val = str[i] - '0'; //0 if 0, 1 if 1
baraki 0:11e8c8e09a07 174 if(val){
baraki 0:11e8c8e09a07 175 int add = (int) pow((float)2.0,(float)i);
baraki 1:86b2752371c2 176 brailleCharacter += add;
baraki 0:11e8c8e09a07 177 }
baraki 0:11e8c8e09a07 178 }
baraki 2:7da645fba3eb 179 //pc.printf("sending: %d\n",(int)brailleCharacter);
baraki 1:86b2752371c2 180 return brailleCharacter;
baraki 0:11e8c8e09a07 181 }
baraki 0:11e8c8e09a07 182
baraki 0:11e8c8e09a07 183 char dist_to_bin(float dist){
baraki 0:11e8c8e09a07 184 char braille[9] = {0};
baraki 0:11e8c8e09a07 185 dist_to_str(dist,braille);
baraki 0:11e8c8e09a07 186 //pc.printf("sending: %s\t",braille);
baraki 0:11e8c8e09a07 187 return str_to_bin(braille);
baraki 0:11e8c8e09a07 188 }
baraki 0:11e8c8e09a07 189
baraki 2:7da645fba3eb 190 //convert old column format to new and improved format
baraki 2:7da645fba3eb 191 //old format:
baraki 2:7da645fba3eb 192 //++++++++++ 0 1
baraki 2:7da645fba3eb 193 //0011223344 1 2
baraki 2:7da645fba3eb 194 //0011223344 2 4
baraki 2:7da645fba3eb 195 //0011223344 3 8
baraki 2:7da645fba3eb 196 //0011223344 4 16
baraki 2:7da645fba3eb 197 //0011223344 5 32
baraki 2:7da645fba3eb 198 //0011223344 6 64
baraki 2:7da645fba3eb 199 //0011223344 7 128
baraki 2:7da645fba3eb 200
baraki 2:7da645fba3eb 201 //new format
baraki 2:7da645fba3eb 202 //0123456789
baraki 2:7da645fba3eb 203
baraki 2:7da645fba3eb 204 //++++++++++ 0 1
baraki 2:7da645fba3eb 205 //--222222-- 1 2
baraki 2:7da645fba3eb 206 //--222222-- 2 4
baraki 2:7da645fba3eb 207 //--222222-- 3 8
baraki 2:7da645fba3eb 208 //1111223333 4 16
baraki 2:7da645fba3eb 209 //1111223333 5 32
baraki 2:7da645fba3eb 210 //0000224444 6 64
baraki 2:7da645fba3eb 211 //0000224444 7 128
baraki 2:7da645fba3eb 212 void convertBrailleChar(char *brailleData){
baraki 2:7da645fba3eb 213 char lidar0 = brailleData[0] >> 4; //shift lidar0,1,3,4 data over by 4 bits (only loooking at nearest 4 data bits)
baraki 2:7da645fba3eb 214 char lidar1 = brailleData[2] >> 4;
baraki 2:7da645fba3eb 215 char lidar2 = brailleData[4];
baraki 2:7da645fba3eb 216 char lidar3 = brailleData[6] >> 4;
baraki 2:7da645fba3eb 217 char lidar4 = brailleData[8] >> 4;
baraki 2:7da645fba3eb 218
baraki 2:7da645fba3eb 219 //clear brailleData
baraki 2:7da645fba3eb 220 for(int i=0;i<10;i++)
baraki 2:7da645fba3eb 221 if(i != 4 && i != 5)
baraki 2:7da645fba3eb 222 brailleData[i] = 1;
baraki 2:7da645fba3eb 223
baraki 2:7da645fba3eb 224 for(int i=0;i<4;i++){ //add lidar0 and lidar1 data to first 4 columns
baraki 2:7da645fba3eb 225 int bit0 = (lidar0 >> i) & 0x01; //we want the 1st bit 1st, 4th bit 4th. Then bit mask to show only the 0x01 bit.
baraki 2:7da645fba3eb 226 brailleData[i] += 64*bit0 + 128*bit0; //add it to bits 6 and 7
baraki 2:7da645fba3eb 227
baraki 2:7da645fba3eb 228 int bit1 = (lidar1 >> i) & 0x01; //lidar 1 (rows 2, 3; columns 0-3
baraki 2:7da645fba3eb 229 brailleData[i] += 32*bit1 + 16*bit1;
baraki 2:7da645fba3eb 230
baraki 2:7da645fba3eb 231 int j = 9-i;
baraki 2:7da645fba3eb 232 int bit4 = (lidar4 >> i) & 0x01; //lidar 4 (rows 0, 1; columns 6-9)
baraki 2:7da645fba3eb 233 brailleData[j] += 64*bit4 + 128*bit4;
baraki 2:7da645fba3eb 234
baraki 2:7da645fba3eb 235 int bit3 = (lidar3 >> i) & 0x01; //lidar 3 (rows 2, 3; columns 6-9)
baraki 2:7da645fba3eb 236 brailleData[j] += 32*bit3 + 16*bit3;
baraki 2:7da645fba3eb 237
baraki 2:7da645fba3eb 238 if(i == 2 || i == 3){ //add lidar 2 data to top 4 rows of columns 2,3 and 6,7
baraki 2:7da645fba3eb 239 uint8_t bit5 = ((lidar2 >> 1) << 5);// 0x00001110; //we are only interested in the smallest 3 bits (not including the LSB)
baraki 2:7da645fba3eb 240 bit5 = bit5/16;
baraki 2:7da645fba3eb 241 pc.printf("lidar2: %d, brailleData: %d, bit5: %d\n",lidar2,brailleData[i],bit5);
baraki 2:7da645fba3eb 242 brailleData[i] += bit5; //no need to scale bits
baraki 2:7da645fba3eb 243 brailleData[j] += bit5;
baraki 2:7da645fba3eb 244 }
baraki 2:7da645fba3eb 245 }
baraki 2:7da645fba3eb 246 // brailleData[4] += (lidar2 & 0x11111110);
baraki 2:7da645fba3eb 247 // brailleData[5] += (lidar2 & 0x11111110);
baraki 2:7da645fba3eb 248
baraki 2:7da645fba3eb 249 }
baraki 2:7da645fba3eb 250
baraki 0:11e8c8e09a07 251 int main()
baraki 0:11e8c8e09a07 252 {
baraki 0:11e8c8e09a07 253 pc.baud(PC_BAUD);
baraki 0:11e8c8e09a07 254 bt.baud(BT_BAUD);
baraki 0:11e8c8e09a07 255 pc.attach(&rxCallback, MODSERIAL::RxIrq);
baraki 0:11e8c8e09a07 256 bt.attach(&txCallback, MODSERIAL::TxIrq);
baraki 0:11e8c8e09a07 257
baraki 0:11e8c8e09a07 258 //set up IMU
baraki 0:11e8c8e09a07 259 imu.reset();
baraki 0:11e8c8e09a07 260 imu.setmode(OPERATION_MODE_NDOF);
baraki 0:11e8c8e09a07 261 setCal();
baraki 0:11e8c8e09a07 262 imu.get_calib();
baraki 0:11e8c8e09a07 263 while (imu.calib == 0)
baraki 0:11e8c8e09a07 264 {
baraki 0:11e8c8e09a07 265 imu.get_calib();
baraki 0:11e8c8e09a07 266 }
baraki 0:11e8c8e09a07 267
baraki 0:11e8c8e09a07 268 sensor.frequency(100000);
baraki 0:11e8c8e09a07 269
baraki 0:11e8c8e09a07 270 char sendData[1] = {0x00};
baraki 0:11e8c8e09a07 271
baraki 0:11e8c8e09a07 272 int addresses[7];
baraki 0:11e8c8e09a07 273 addresses[0] = 0x60; //0x60
baraki 0:11e8c8e09a07 274 addresses[1] = 0x64; //0x64
baraki 0:11e8c8e09a07 275 addresses[2] = 0x68; //middle
baraki 0:11e8c8e09a07 276 addresses[3] = 0x6C;
baraki 0:11e8c8e09a07 277 addresses[4] = 0x70;
baraki 0:11e8c8e09a07 278 addresses[5] = 0x80; //up
baraki 0:11e8c8e09a07 279 addresses[6] = 0x84; //down
baraki 0:11e8c8e09a07 280
baraki 0:11e8c8e09a07 281 uint8_t pulses[7] = {0};
baraki 0:11e8c8e09a07 282 uint8_t intensity[7] = {0};
baraki 0:11e8c8e09a07 283
baraki 0:11e8c8e09a07 284 char btData[12] = {'a','b','c','d','e','f','g','\n','\0'};
baraki 0:11e8c8e09a07 285
baraki 0:11e8c8e09a07 286 //calibrate down sensor
baraki 0:11e8c8e09a07 287 int down_cal = 0;
baraki 0:11e8c8e09a07 288 int down_cal2 = 0;
baraki 0:11e8c8e09a07 289 float cospi = 0; //"cosine of initial pitch"
baraki 0:11e8c8e09a07 290
baraki 0:11e8c8e09a07 291 unsigned int i = 0;
baraki 0:11e8c8e09a07 292 int count = 0; //for calibration
baraki 0:11e8c8e09a07 293 int count2 = 0;//for averaging
baraki 0:11e8c8e09a07 294 int count2_2 = 0; //for averaging second down sensor
baraki 0:11e8c8e09a07 295 int differenceAvgSum = 0;
baraki 0:11e8c8e09a07 296 int differenceAvgSum2 = 0;
baraki 0:11e8c8e09a07 297 int moving_ave[5] = {0};
baraki 0:11e8c8e09a07 298 int moving_ave2[5] = {0};
baraki 0:11e8c8e09a07 299 while (1) {
baraki 0:11e8c8e09a07 300 for(int k=0; k<5; k++) {
baraki 0:11e8c8e09a07 301 char receiveData[3] = {0};
baraki 0:11e8c8e09a07 302 if(sensor.write(addresses[k], sendData, 1)){
baraki 0:11e8c8e09a07 303 //pc.printf("writing to sensor %d failed\n", k);
baraki 0:11e8c8e09a07 304 }
baraki 0:11e8c8e09a07 305 //write ---> 0 on success, 1 on failure
baraki 0:11e8c8e09a07 306 i = 0;
baraki 0:11e8c8e09a07 307 while(sensor.read(addresses[k], receiveData, 3) && i < 10) {
baraki 0:11e8c8e09a07 308 i++;
baraki 0:11e8c8e09a07 309 //pc.printf("reading from sensor %d failed\n",k);
baraki 0:11e8c8e09a07 310 }
baraki 0:11e8c8e09a07 311 //while(!twi_master_transfer(addresses[k], sendData, 1, TWI_ISSUE_STOP)){;}
baraki 0:11e8c8e09a07 312 //while(!twi_master_transfer(addresses[k] + 1, receiveData, 3, TWI_ISSUE_STOP)){;}
baraki 0:11e8c8e09a07 313 int distance = ((int)receiveData[0]<<8 )+ (int)receiveData[1];
baraki 0:11e8c8e09a07 314 if(brailleMode) {
baraki 0:11e8c8e09a07 315 brailleChar[2*k] = dist_to_bin(((float)distance));
baraki 0:11e8c8e09a07 316 brailleChar[2*k+1] = brailleChar[2*k];
baraki 0:11e8c8e09a07 317 //if(k == 4)
baraki 0:11e8c8e09a07 318 // pc.printf("\n");
baraki 0:11e8c8e09a07 319 } else {
baraki 0:11e8c8e09a07 320 float radius[4] = {0};
baraki 0:11e8c8e09a07 321 angleToRadius(radius, angle[k]);
baraki 0:11e8c8e09a07 322 if(distance == 0) {
baraki 0:11e8c8e09a07 323 pulses[k] = 1;
baraki 0:11e8c8e09a07 324 intensity[k] = 0;
baraki 0:11e8c8e09a07 325 }
baraki 0:11e8c8e09a07 326 //for(int z=0;z<4;z++)
baraki 0:11e8c8e09a07 327 // pc.printf("angle: %f radius %d is %f ",angle[k],z,radius[z]);
baraki 0:11e8c8e09a07 328 //pc.printf("\n");
baraki 0:11e8c8e09a07 329 if(distance > 0 && distance < radius[0]) {
baraki 0:11e8c8e09a07 330 pulses[k] = 5;
baraki 0:11e8c8e09a07 331 intensity[k] = 7;
baraki 0:11e8c8e09a07 332 } else if(distance >= radius[0] && distance < radius[1]) {
baraki 0:11e8c8e09a07 333 pulses[k] = 4;
baraki 0:11e8c8e09a07 334 intensity[k] = 6;
baraki 0:11e8c8e09a07 335 } else if(distance >= radius[1] && distance < radius[2]) {
baraki 0:11e8c8e09a07 336 pulses[k] = 3;
baraki 0:11e8c8e09a07 337 intensity[k] = 5;
baraki 0:11e8c8e09a07 338 } else if(distance >= radius[2] && distance < radius[3]) {
baraki 0:11e8c8e09a07 339 pulses[k] = 2;
baraki 0:11e8c8e09a07 340 intensity[k] = 2;
baraki 0:11e8c8e09a07 341 } else if(distance >= radius[3]) {
baraki 0:11e8c8e09a07 342 pulses[k] = 1;
baraki 0:11e8c8e09a07 343 intensity[k] = 0;
baraki 0:11e8c8e09a07 344 }
baraki 0:11e8c8e09a07 345 //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",k,pulses[k],intensity[k]);
baraki 0:11e8c8e09a07 346 }
baraki 0:11e8c8e09a07 347 }
baraki 0:11e8c8e09a07 348
baraki 0:11e8c8e09a07 349 //find UP distance
baraki 0:11e8c8e09a07 350 // char receiveData2[3] = {0};
baraki 0:11e8c8e09a07 351 // sensor.write(addresses[5], sendData, 1);
baraki 0:11e8c8e09a07 352 // i = 0;
baraki 0:11e8c8e09a07 353 // while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
baraki 0:11e8c8e09a07 354 // i++;}
baraki 0:11e8c8e09a07 355 // int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
baraki 0:11e8c8e09a07 356 // if(distance2 >= UP_MIN && distance2 < UP_MAX) {
baraki 0:11e8c8e09a07 357 // pulses[5] = 5; ///TODO WAS: 5
baraki 0:11e8c8e09a07 358 // intensity[5] = 7; ///TODO: 7
baraki 0:11e8c8e09a07 359 // } else {
baraki 0:11e8c8e09a07 360 // pulses[5] = 1;
baraki 0:11e8c8e09a07 361 // intensity[5] = 0;
baraki 0:11e8c8e09a07 362 // }
baraki 0:11e8c8e09a07 363
baraki 0:11e8c8e09a07 364 imu.get_angles();
baraki 0:11e8c8e09a07 365 float pitch = imu.euler.pitch;
baraki 0:11e8c8e09a07 366 //pc.printf("pitch: %f\n",pitch);
baraki 0:11e8c8e09a07 367
baraki 0:11e8c8e09a07 368 //NEW down sensor:
baraki 0:11e8c8e09a07 369 char receiveData2[3] = {0};
baraki 0:11e8c8e09a07 370 sensor.write(addresses[5], sendData, 1);
baraki 0:11e8c8e09a07 371 i = 0;
baraki 0:11e8c8e09a07 372 while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
baraki 0:11e8c8e09a07 373 i++;}
baraki 0:11e8c8e09a07 374 int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
baraki 0:11e8c8e09a07 375 // if(count > 50) { //calibration over
baraki 0:11e8c8e09a07 376 // pc.printf("measuring up\n");
baraki 0:11e8c8e09a07 377 // float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
baraki 0:11e8c8e09a07 378 // float allowX = ALLOW_HEIGHT/cosp;
baraki 0:11e8c8e09a07 379 // float new_down_cal = ((float)down_cal2)*cospi/cosp;
baraki 0:11e8c8e09a07 380 //
baraki 0:11e8c8e09a07 381 // //use moving average to find deltaX
baraki 0:11e8c8e09a07 382 // int difference = abs(new_down_cal - distance2);
baraki 0:11e8c8e09a07 383 // differenceAvgSum2 = differenceAvgSum2 - moving_ave2[count2_2];
baraki 0:11e8c8e09a07 384 // moving_ave2[count2_2] = difference;
baraki 0:11e8c8e09a07 385 // differenceAvgSum2 = differenceAvgSum2 + difference;
baraki 0:11e8c8e09a07 386 // count2_2 = count2_2 + 1;
baraki 0:11e8c8e09a07 387 // int ave = (int)(differenceAvgSum2/5);
baraki 0:11e8c8e09a07 388 //
baraki 0:11e8c8e09a07 389 // //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 0:11e8c8e09a07 390 //
baraki 0:11e8c8e09a07 391 // //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
baraki 0:11e8c8e09a07 392 // if(ave >= allowX) {
baraki 0:11e8c8e09a07 393 // pulses[5] = 5;
baraki 0:11e8c8e09a07 394 // intensity[5] = 7;
baraki 0:11e8c8e09a07 395 // } else {
baraki 0:11e8c8e09a07 396 // pulses[5] = 1;
baraki 0:11e8c8e09a07 397 // intensity[5] = 0;
baraki 0:11e8c8e09a07 398 // }
baraki 0:11e8c8e09a07 399 //
baraki 0:11e8c8e09a07 400 // if(count2_2 >4) {
baraki 0:11e8c8e09a07 401 // count2 = 0;
baraki 0:11e8c8e09a07 402 // }
baraki 0:11e8c8e09a07 403 // }
baraki 0:11e8c8e09a07 404 // else{
baraki 0:11e8c8e09a07 405 // pulses[5] = 1;
baraki 0:11e8c8e09a07 406 // intensity[5] = 0;
baraki 0:11e8c8e09a07 407 // down_cal2 = distance2;
baraki 0:11e8c8e09a07 408 // }
baraki 0:11e8c8e09a07 409
baraki 0:11e8c8e09a07 410 //find DOWN distance
baraki 0:11e8c8e09a07 411 char receiveData3[3] = {0};
baraki 0:11e8c8e09a07 412 i = 0;
baraki 0:11e8c8e09a07 413 sensor.write(addresses[6], sendData, 1);
baraki 0:11e8c8e09a07 414 while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){
baraki 0:11e8c8e09a07 415 i++;}
baraki 0:11e8c8e09a07 416 int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
baraki 0:11e8c8e09a07 417 if(count > 50) { //calibration over
baraki 0:11e8c8e09a07 418 //get allowableX and adjusted down_cal from IMU
baraki 0:11e8c8e09a07 419 float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
baraki 0:11e8c8e09a07 420 float allowX = ALLOW_HEIGHT/cosp;
baraki 0:11e8c8e09a07 421 float new_down_cal = ((float)down_cal)*cospi/cosp;
baraki 0:11e8c8e09a07 422
baraki 0:11e8c8e09a07 423 float new_down_cal2 = ((float)down_cal2)*cospi/cosp;
baraki 0:11e8c8e09a07 424
baraki 0:11e8c8e09a07 425 //use moving average to find deltaX
baraki 0:11e8c8e09a07 426 int difference = abs(new_down_cal - distance3);
baraki 0:11e8c8e09a07 427 differenceAvgSum = differenceAvgSum - moving_ave[count2];
baraki 0:11e8c8e09a07 428 moving_ave[count2] = difference;
baraki 0:11e8c8e09a07 429 differenceAvgSum = differenceAvgSum + difference;
baraki 0:11e8c8e09a07 430 int ave = (int)(differenceAvgSum/5);
baraki 0:11e8c8e09a07 431
baraki 0:11e8c8e09a07 432 int difference2 = abs(new_down_cal2 - distance2);
baraki 0:11e8c8e09a07 433 differenceAvgSum2 = differenceAvgSum2 - moving_ave2[count2];
baraki 0:11e8c8e09a07 434 moving_ave2[count2] = difference2;
baraki 0:11e8c8e09a07 435 differenceAvgSum2 = differenceAvgSum2 + difference2;
baraki 0:11e8c8e09a07 436 count2 = count2 + 1;
baraki 0:11e8c8e09a07 437 int ave2 = (int)(differenceAvgSum2/5);
baraki 0:11e8c8e09a07 438
baraki 0:11e8c8e09a07 439 count2 = count2 + 1;
baraki 0:11e8c8e09a07 440
baraki 0:11e8c8e09a07 441 //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 0:11e8c8e09a07 442
baraki 0:11e8c8e09a07 443 //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
baraki 0:11e8c8e09a07 444 if(!brailleMode) {
baraki 0:11e8c8e09a07 445 if(ave >= allowX) {
baraki 0:11e8c8e09a07 446 pulses[6] = 5;
baraki 0:11e8c8e09a07 447 intensity[6] = 7;
baraki 0:11e8c8e09a07 448 } else {
baraki 0:11e8c8e09a07 449 pulses[6] = 1;
baraki 0:11e8c8e09a07 450 intensity[6] = 0;
baraki 0:11e8c8e09a07 451 }
baraki 0:11e8c8e09a07 452
baraki 0:11e8c8e09a07 453 if(ave2 >= allowX) {
baraki 0:11e8c8e09a07 454 pulses[5] = 5;
baraki 0:11e8c8e09a07 455 intensity[5] = 7;
baraki 0:11e8c8e09a07 456 } else {
baraki 0:11e8c8e09a07 457 pulses[5] = 1;
baraki 0:11e8c8e09a07 458 intensity[5] = 0;
baraki 0:11e8c8e09a07 459 }
baraki 0:11e8c8e09a07 460 }
baraki 0:11e8c8e09a07 461
baraki 0:11e8c8e09a07 462 if(brailleMode){
baraki 0:11e8c8e09a07 463 if(ave >= allowX || ave2 >= allowX){
baraki 0:11e8c8e09a07 464 brailleChar[4] = dist_to_bin(10.0);
baraki 0:11e8c8e09a07 465 brailleChar[5] = brailleChar[4];
baraki 0:11e8c8e09a07 466 }
baraki 0:11e8c8e09a07 467 }
baraki 0:11e8c8e09a07 468
baraki 0:11e8c8e09a07 469 if(count2 >4) {
baraki 0:11e8c8e09a07 470 count2 = 0;
baraki 0:11e8c8e09a07 471 }
baraki 0:11e8c8e09a07 472 } else {
baraki 0:11e8c8e09a07 473 down_cal2 = distance2;
baraki 0:11e8c8e09a07 474 down_cal = distance3;
baraki 0:11e8c8e09a07 475 cospi = cos((pitch-DOWN_ANGLE) * PI/180.0);
baraki 0:11e8c8e09a07 476 count = count+1;
baraki 0:11e8c8e09a07 477 }
baraki 0:11e8c8e09a07 478 //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",6,pulses[6],intensity[6]);
baraki 0:11e8c8e09a07 479
baraki 0:11e8c8e09a07 480 //pc.printf("about to send data\n");
baraki 0:11e8c8e09a07 481 if(!brailleMode){
baraki 0:11e8c8e09a07 482 btData[0] = (pulses[0] << 5) | (intensity[0] << 2);
baraki 0:11e8c8e09a07 483 btData[1] = (pulses[1] << 4) | (intensity[1] << 1);
baraki 0:11e8c8e09a07 484 btData[2] = (pulses[2] << 3) | (intensity[2]);
baraki 0:11e8c8e09a07 485 btData[3] = (pulses[3] << 2) | (intensity[3] >> 1);
baraki 0:11e8c8e09a07 486 btData[4] = (intensity[3] << 7) | (pulses[4] << 1) | (intensity[4] >> 2);
baraki 0:11e8c8e09a07 487 btData[5] = (intensity[4] << 6) | (0x3);
baraki 0:11e8c8e09a07 488 btData[6] = (pulses[5] << 5) | (intensity[5] << 2);
baraki 0:11e8c8e09a07 489 btData[7] = (pulses[6] << 5) | (intensity[6] << 2);
baraki 0:11e8c8e09a07 490 btData[8] = '\0';
baraki 0:11e8c8e09a07 491 for(int j=0; j<9; j++) {
baraki 0:11e8c8e09a07 492 if(bt.writeable())
baraki 0:11e8c8e09a07 493 bt.putc(btData[j]);
baraki 0:11e8c8e09a07 494 //wait(0.001);
baraki 0:11e8c8e09a07 495 }
baraki 0:11e8c8e09a07 496 }
baraki 0:11e8c8e09a07 497 else{
baraki 2:7da645fba3eb 498 convertBrailleChar(brailleChar);
baraki 2:7da645fba3eb 499
baraki 0:11e8c8e09a07 500 if(bt.writeable()){
baraki 1:86b2752371c2 501 // brailleChar[0] = 1;
baraki 1:86b2752371c2 502 // brailleChar[1] = 1+2;
baraki 1:86b2752371c2 503 // brailleChar[2] = 1+2+4;
baraki 1:86b2752371c2 504 // brailleChar[3] = 1+2+4+8;
baraki 1:86b2752371c2 505 // brailleChar[4] = 1+2+4+8+16;
baraki 1:86b2752371c2 506 // brailleChar[5] = 1+2+4+8+16+32;
baraki 1:86b2752371c2 507 // brailleChar[6] = 1+2+4+8+16+32+64;
baraki 1:86b2752371c2 508 // brailleChar[7] = 1+2+4+16+32+64+128;
baraki 1:86b2752371c2 509 // brailleChar[8] = 1+2+4;
baraki 1:86b2752371c2 510 // brailleChar[9] = 1+2;
baraki 0:11e8c8e09a07 511 for(int i=0;i<10;i++){
baraki 0:11e8c8e09a07 512 bt.putc(brailleChar[i]);
baraki 0:11e8c8e09a07 513 }
baraki 0:11e8c8e09a07 514 while(!bt.writeable()){}
baraki 0:11e8c8e09a07 515 bt.putc('\0');
baraki 0:11e8c8e09a07 516 }
baraki 0:11e8c8e09a07 517 wait(0.2);
baraki 0:11e8c8e09a07 518 }
baraki 0:11e8c8e09a07 519
baraki 0:11e8c8e09a07 520 wait(0.05);
baraki 0:11e8c8e09a07 521 //pc.printf("finished sending data\n");
baraki 0:11e8c8e09a07 522 //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9);
baraki 0:11e8c8e09a07 523 }
baraki 0:11e8c8e09a07 524 }