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@15:d6edd100d7fe, 2015-10-17 (annotated)

Committer:
baraki
Date:
Sat Oct 17 19:30:41 2015 +0000
Revision:
15:d6edd100d7fe
Parent:
14:8620dceb3ba2
Child:
16:70fce771d828
changed up sensor to second down sensor (not sure if it works)

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
baraki 14:8620dceb3ba2 13 float MIN_MIN_A=200.0; //min side range
baraki 14:8620dceb3ba2 14 float MAX_MIN_A=500.0; //min side range
baraki 14:8620dceb3ba2 15 float MIN_MAX_A=400.0; //max side range
baraki 14:8620dceb3ba2 16 float MAX_MAX_A=1200.0; //max side range
baraki 14:8620dceb3ba2 17
baraki 14:8620dceb3ba2 18 float MIN_MIN_B=400.0; //min front range
baraki 14:8620dceb3ba2 19 float MAX_MIN_B=1000.0; //min front range
baraki 14:8620dceb3ba2 20 float MIN_MAX_B=1200.0; //max front range
baraki 14:8620dceb3ba2 21 float MAX_MAX_B=2750.0; //max front range
baraki 14:8620dceb3ba2 22
rkk 5:d4ed744beea2 23 //Initial LONG Range Settings
rkk 13:b22fd9c4fbb4 24 //#define RANGE_01 750
rkk 13:b22fd9c4fbb4 25 //#define RANGE_02 1100
rkk 13:b22fd9c4fbb4 26 //#define RANGE_03 1700
rkk 13:b22fd9c4fbb4 27 //#define RANGE_04 2750
rkk 13:b22fd9c4fbb4 28 //#define UP_MIN 600
rkk 13:b22fd9c4fbb4 29 //#define UP_MAX 1300
rkk 5:d4ed744beea2 30
rkk 5:d4ed744beea2 31 // Mid range settings
rkk 13:b22fd9c4fbb4 32 #define RANGE_01 450
rkk 13:b22fd9c4fbb4 33 #define RANGE_02 750
rkk 13:b22fd9c4fbb4 34 #define RANGE_03 1150
rkk 13:b22fd9c4fbb4 35 #define RANGE_04 1550
rkk 5:d4ed744beea2 36
rkk 5:d4ed744beea2 37 // Short range settings
rkk 5:d4ed744beea2 38 //#define RANGE_01 400
rkk 5:d4ed744beea2 39 //#define RANGE_02 650
rkk 5:d4ed744beea2 40 //#define RANGE_03 1000
rkk 5:d4ed744beea2 41 //#define RANGE_04 1350
rkk 5:d4ed744beea2 42
rkk 5:d4ed744beea2 43
baraki 0:ce4f790399d9 44 #define PC_BAUD 9600
rkk 4:c53761262e3f 45 #define BT_BAUD 115200
baraki 0:ce4f790399d9 46 #define TX_PIN p13
baraki 0:ce4f790399d9 47 #define RX_PIN p14
baraki 0:ce4f790399d9 48 #define SDA_PIN p9 //SDA pin on LPC1768
baraki 0:ce4f790399d9 49 #define SCL_PIN p10 //SCL pin on LPC1768
baraki 7:660e8ddb231e 50 #define IMU_SDA p28
baraki 7:660e8ddb231e 51 #define IMU_SCL p27
baraki 7:660e8ddb231e 52 #define PI 3.14159265
baraki 0:ce4f790399d9 53
baraki 14:8620dceb3ba2 54 float angle[5] = {PI, 3*PI/4, PI/2, PI/4, 0}; //relates lidar number to its angle in radians
baraki 14:8620dceb3ba2 55
baraki 7:660e8ddb231e 56 BNO055 imu(p28,p27);
rkk 5:d4ed744beea2 57
baraki 0:ce4f790399d9 58 I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1
baraki 0:ce4f790399d9 59 MODSERIAL bt(TX_PIN, RX_PIN);
baraki 0:ce4f790399d9 60 MODSERIAL pc(USBTX,USBRX);
baraki 14:8620dceb3ba2 61 AnalogIn potentiometer(p18); //doesn't exist yet
baraki 0:ce4f790399d9 62
rkk 13:b22fd9c4fbb4 63
baraki 7:660e8ddb231e 64 //for calibrating IMU
baraki 9:59d23ab8d73b 65 //for IMU1:
baraki 9:59d23ab8d73b 66 //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 67 //for IMU2:
baraki 9:59d23ab8d73b 68 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 69
baraki 7:660e8ddb231e 70
baraki 7:660e8ddb231e 71 //for encoder:
baraki 7:660e8ddb231e 72 //need to set pins to DigitalIn with internal pullup resistors
baraki 7:660e8ddb231e 73 //DigitalIn mySwitch(p21);
baraki 7:660e8ddb231e 74 //mySwitch.mode(PullUp);
baraki 7:660e8ddb231e 75
baraki 0:ce4f790399d9 76 bool newline_detected = false;
baraki 0:ce4f790399d9 77 bool newline_sent = false;
baraki 0:ce4f790399d9 78
baraki 14:8620dceb3ba2 79 float potValue(){
baraki 15:d6edd100d7fe 80 //float pot = potentiometer.read();
baraki 15:d6edd100d7fe 81 float pot = 0.25;
baraki 14:8620dceb3ba2 82 if(pot <= 0.5)
baraki 14:8620dceb3ba2 83 return 2*pot;
baraki 14:8620dceb3ba2 84 else
baraki 14:8620dceb3ba2 85 return 2*(1-pot); //if pot = 1, return 0. if pot = 0.5, return 1
baraki 14:8620dceb3ba2 86 }
baraki 14:8620dceb3ba2 87
baraki 14:8620dceb3ba2 88 void angleToRadius(float* radius,float angle){
baraki 14:8620dceb3ba2 89 //a is horizontal semi-axis
baraki 14:8620dceb3ba2 90 //b is vertical semi-axis
baraki 14:8620dceb3ba2 91 //scale between MIN_A, MAX_A, MIN_B, MAX_B
baraki 14:8620dceb3ba2 92 //using pot value
baraki 14:8620dceb3ba2 93 float pot = potValue();
baraki 14:8620dceb3ba2 94 //define new min_a scaled between MIN_MIN_A and MAX_MIN_A
baraki 14:8620dceb3ba2 95 float min_a = MIN_MIN_A + (MAX_MIN_A - MIN_MIN_A)*pot;
baraki 14:8620dceb3ba2 96 //define new max_a scaled between MIN_MAX_A and MAX_MAX_A
baraki 14:8620dceb3ba2 97 float max_a = MIN_MAX_A + (MAX_MAX_A - MIN_MAX_A)*pot;
baraki 14:8620dceb3ba2 98 //define new min_b between MIN_MIN_B and MAX_MIN_B
baraki 14:8620dceb3ba2 99 float min_b = MIN_MIN_B + (MAX_MIN_B - MIN_MIN_B)*pot;
baraki 14:8620dceb3ba2 100 //define new max_b scaled between MIN_MAX_B and MAX_MAX_B
baraki 14:8620dceb3ba2 101 float max_b = MIN_MAX_B + (MAX_MAX_B - MIN_MAX_B)*pot;
baraki 14:8620dceb3ba2 102
baraki 14:8620dceb3ba2 103 float scale[4] = {0, 0.3, 0.6,1.0};
baraki 14:8620dceb3ba2 104 for(int i=0;i<4;i++){
baraki 14:8620dceb3ba2 105 //scale a and be to be somewhere between the min and max based on i
baraki 14:8620dceb3ba2 106 // i = 0 -- min
baraki 14:8620dceb3ba2 107 // i = 5 -- max
baraki 14:8620dceb3ba2 108 float a = min_a + (max_a - min_a)*scale[i];
baraki 14:8620dceb3ba2 109 float b = min_b + (max_b - min_b)*scale[i];
baraki 14:8620dceb3ba2 110 radius[i] = a*b/sqrt(pow(a,2)*pow(sin(angle),2) + pow(b,2)*pow(cos(angle),2));
baraki 14:8620dceb3ba2 111 }
baraki 14:8620dceb3ba2 112 }
baraki 14:8620dceb3ba2 113
baraki 7:660e8ddb231e 114 void setCal(){
baraki 7:660e8ddb231e 115 imu.write_calibration_data();
baraki 7:660e8ddb231e 116 }
baraki 7:660e8ddb231e 117
baraki 0:ce4f790399d9 118 // Called everytime a new character goes into
baraki 0:ce4f790399d9 119 // the RX buffer. Test that character for \n
baraki 0:ce4f790399d9 120 // Note, rxGetLastChar() gets the last char that
baraki 0:ce4f790399d9 121 // we received but it does NOT remove it from
baraki 0:ce4f790399d9 122 // the RX buffer.
baraki 0:ce4f790399d9 123 void rxCallback(MODSERIAL_IRQ_INFO *q)
baraki 0:ce4f790399d9 124 {
baraki 0:ce4f790399d9 125 MODSERIAL *serial = q->serial;
baraki 0:ce4f790399d9 126 if ( serial->rxGetLastChar() == '\n') {
baraki 0:ce4f790399d9 127 newline_detected = true;
baraki 0:ce4f790399d9 128 }
baraki 0:ce4f790399d9 129
baraki 0:ce4f790399d9 130 }
baraki 0:ce4f790399d9 131
baraki 0:ce4f790399d9 132 void txCallback(MODSERIAL_IRQ_INFO *q)
baraki 0:ce4f790399d9 133 {
baraki 0:ce4f790399d9 134 MODSERIAL *serial = q->serial;
baraki 0:ce4f790399d9 135 if ( serial->txGetLastChar() == '\0') {
baraki 0:ce4f790399d9 136 newline_sent = true;
baraki 0:ce4f790399d9 137 }
baraki 0:ce4f790399d9 138 }
baraki 0:ce4f790399d9 139
baraki 0:ce4f790399d9 140 int main()
baraki 0:ce4f790399d9 141 {
baraki 0:ce4f790399d9 142 pc.baud(PC_BAUD);
baraki 0:ce4f790399d9 143 bt.baud(BT_BAUD);
baraki 0:ce4f790399d9 144 pc.attach(&rxCallback, MODSERIAL::RxIrq);
baraki 0:ce4f790399d9 145 bt.attach(&txCallback, MODSERIAL::TxIrq);
baraki 0:ce4f790399d9 146
baraki 7:660e8ddb231e 147 //set up IMU
baraki 7:660e8ddb231e 148 imu.reset();
baraki 7:660e8ddb231e 149 imu.setmode(OPERATION_MODE_NDOF);
baraki 7:660e8ddb231e 150 setCal();
baraki 7:660e8ddb231e 151 imu.get_calib();
baraki 7:660e8ddb231e 152 while (imu.calib == 0)
baraki 7:660e8ddb231e 153 {
baraki 7:660e8ddb231e 154 imu.get_calib();
baraki 7:660e8ddb231e 155 }
baraki 7:660e8ddb231e 156
baraki 0:ce4f790399d9 157 sensor.frequency(100000);
baraki 0:ce4f790399d9 158
baraki 0:ce4f790399d9 159 char sendData[1] = {0x00};
baraki 0:ce4f790399d9 160
baraki 0:ce4f790399d9 161 int addresses[7];
baraki 0:ce4f790399d9 162 addresses[0] = 0x60; //0x60
baraki 0:ce4f790399d9 163 addresses[1] = 0x64; //0x64
baraki 0:ce4f790399d9 164 addresses[2] = 0x68; //middle
baraki 0:ce4f790399d9 165 addresses[3] = 0x6C;
baraki 0:ce4f790399d9 166 addresses[4] = 0x70;
baraki 0:ce4f790399d9 167 addresses[5] = 0x80; //up
baraki 0:ce4f790399d9 168 addresses[6] = 0x84; //down
baraki 0:ce4f790399d9 169
baraki 0:ce4f790399d9 170 uint8_t pulses[7] = {0};
baraki 0:ce4f790399d9 171 uint8_t intensity[7] = {0};
baraki 0:ce4f790399d9 172
baraki 0:ce4f790399d9 173 char btData[12] = {'a','b','c','d','e','f','g','\n','\0'};
baraki 0:ce4f790399d9 174
baraki 0:ce4f790399d9 175 //calibrate down sensor
baraki 0:ce4f790399d9 176 int down_cal = 0;
baraki 15:d6edd100d7fe 177 int down_cal2 = 0;
baraki 8:2ddeec5d8f84 178 float cospi = 0; //"cosine of initial pitch"
baraki 0:ce4f790399d9 179
baraki 0:ce4f790399d9 180 unsigned int i = 0;
baraki 0:ce4f790399d9 181 int count = 0; //for calibration
baraki 0:ce4f790399d9 182 int count2 = 0;//for averaging
baraki 15:d6edd100d7fe 183 int count2_2 = 0; //for averaging second down sensor
baraki 0:ce4f790399d9 184 int differenceAvgSum = 0;
baraki 15:d6edd100d7fe 185 int differenceAvgSum2 = 0;
baraki 0:ce4f790399d9 186 int moving_ave[5] = {0};
baraki 15:d6edd100d7fe 187 int moving_ave2[5] = {0};
baraki 0:ce4f790399d9 188 while (1) {
baraki 0:ce4f790399d9 189 for(int k=0; k<5; k++) {
baraki 0:ce4f790399d9 190 char receiveData[3] = {0};
baraki 0:ce4f790399d9 191 if(sensor.write(addresses[k], sendData, 1)){
baraki 1:5b1d88d69aa2 192 //pc.printf("writing to sensor %d failed\n", k);
baraki 0:ce4f790399d9 193 }
baraki 0:ce4f790399d9 194 //write ---> 0 on success, 1 on failure
baraki 0:ce4f790399d9 195 i = 0;
baraki 2:ec53792aef80 196 while(sensor.read(addresses[k], receiveData, 3) && i < 10) {
baraki 0:ce4f790399d9 197 i++;
baraki 1:5b1d88d69aa2 198 //pc.printf("reading from sensor %d failed\n",k);
baraki 1:5b1d88d69aa2 199 }
baraki 0:ce4f790399d9 200 //while(!twi_master_transfer(addresses[k], sendData, 1, TWI_ISSUE_STOP)){;}
baraki 0:ce4f790399d9 201 //while(!twi_master_transfer(addresses[k] + 1, receiveData, 3, TWI_ISSUE_STOP)){;}
baraki 0:ce4f790399d9 202 int distance = ((int)receiveData[0]<<8 )+ (int)receiveData[1];
baraki 14:8620dceb3ba2 203 float radius[4] = {0};
baraki 14:8620dceb3ba2 204 angleToRadius(radius, angle[k]);
baraki 0:ce4f790399d9 205 if(distance == 0){
baraki 0:ce4f790399d9 206 pulses[k] = 1;
baraki 0:ce4f790399d9 207 intensity[k] = 0;
baraki 0:ce4f790399d9 208 }
baraki 15:d6edd100d7fe 209 //for(int z=0;z<4;z++)
baraki 15:d6edd100d7fe 210 // pc.printf("angle: %f radius %d is %f ",angle[k],z,radius[z]);
baraki 15:d6edd100d7fe 211 //pc.printf("\n");
baraki 14:8620dceb3ba2 212 if(distance > 0 && distance < radius[0]) {
baraki 0:ce4f790399d9 213 pulses[k] = 5;
baraki 0:ce4f790399d9 214 intensity[k] = 7;
baraki 14:8620dceb3ba2 215 } else if(distance >= radius[0] && distance < radius[1]) {
baraki 0:ce4f790399d9 216 pulses[k] = 4;
baraki 0:ce4f790399d9 217 intensity[k] = 6;
baraki 14:8620dceb3ba2 218 } else if(distance >= radius[1] && distance < radius[2]) {
baraki 0:ce4f790399d9 219 pulses[k] = 3;
baraki 0:ce4f790399d9 220 intensity[k] = 5;
baraki 14:8620dceb3ba2 221 } else if(distance >= radius[2] && distance < radius[3]) {
baraki 0:ce4f790399d9 222 pulses[k] = 2;
baraki 0:ce4f790399d9 223 intensity[k] = 2;
baraki 14:8620dceb3ba2 224 } else if(distance >= radius[3]) {
baraki 0:ce4f790399d9 225 pulses[k] = 1;
baraki 0:ce4f790399d9 226 intensity[k] = 0;
baraki 0:ce4f790399d9 227 }
baraki 3:a0ccaf565e8d 228 //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",k,pulses[k],intensity[k]);
baraki 0:ce4f790399d9 229 }
baraki 0:ce4f790399d9 230
baraki 0:ce4f790399d9 231 //find UP distance
baraki 15:d6edd100d7fe 232 // char receiveData2[3] = {0};
baraki 15:d6edd100d7fe 233 // sensor.write(addresses[5], sendData, 1);
baraki 15:d6edd100d7fe 234 // i = 0;
baraki 15:d6edd100d7fe 235 // while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
baraki 15:d6edd100d7fe 236 // i++;}
baraki 15:d6edd100d7fe 237 // int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
baraki 15:d6edd100d7fe 238 // if(distance2 >= UP_MIN && distance2 < UP_MAX) {
baraki 15:d6edd100d7fe 239 // pulses[5] = 5; ///TODO WAS: 5
baraki 15:d6edd100d7fe 240 // intensity[5] = 7; ///TODO: 7
baraki 15:d6edd100d7fe 241 // } else {
baraki 15:d6edd100d7fe 242 // pulses[5] = 1;
baraki 15:d6edd100d7fe 243 // intensity[5] = 0;
baraki 15:d6edd100d7fe 244 // }
baraki 15:d6edd100d7fe 245
baraki 15:d6edd100d7fe 246 //NEW down sensor:
baraki 0:ce4f790399d9 247 char receiveData2[3] = {0};
baraki 0:ce4f790399d9 248 sensor.write(addresses[5], sendData, 1);
baraki 0:ce4f790399d9 249 i = 0;
baraki 2:ec53792aef80 250 while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
baraki 0:ce4f790399d9 251 i++;}
baraki 0:ce4f790399d9 252 int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
baraki 15:d6edd100d7fe 253 if(count > 50) { //calibration over
baraki 15:d6edd100d7fe 254 //get allowableX and adjusted down_cal from IMU
baraki 15:d6edd100d7fe 255 imu.get_angles();
baraki 15:d6edd100d7fe 256 float pitch = imu.euler.pitch;
baraki 15:d6edd100d7fe 257 float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
baraki 15:d6edd100d7fe 258 float allowX = ALLOW_HEIGHT/cosp;
baraki 15:d6edd100d7fe 259 float new_down_cal = ((float)down_cal2)*cospi/cosp;
baraki 15:d6edd100d7fe 260
baraki 15:d6edd100d7fe 261 //use moving average to find deltaX
baraki 15:d6edd100d7fe 262 int difference = abs(new_down_cal - distance2);
baraki 15:d6edd100d7fe 263 differenceAvgSum2 = differenceAvgSum2 - moving_ave2[count2_2];
baraki 15:d6edd100d7fe 264 moving_ave2[count2_2] = difference;
baraki 15:d6edd100d7fe 265 differenceAvgSum2 = differenceAvgSum2 + difference;
baraki 15:d6edd100d7fe 266 count2_2 = count2_2 + 1;
baraki 15:d6edd100d7fe 267 int ave = (int)(differenceAvgSum2/5);
baraki 15:d6edd100d7fe 268
baraki 15:d6edd100d7fe 269 //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 15:d6edd100d7fe 270
baraki 15:d6edd100d7fe 271 //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
baraki 15:d6edd100d7fe 272 if(ave >= allowX) {
baraki 15:d6edd100d7fe 273 pulses[5] = 5;
baraki 15:d6edd100d7fe 274 intensity[5] = 7;
baraki 15:d6edd100d7fe 275 } else {
baraki 15:d6edd100d7fe 276 pulses[5] = 1;
baraki 15:d6edd100d7fe 277 intensity[5] = 0;
baraki 15:d6edd100d7fe 278 }
baraki 15:d6edd100d7fe 279
baraki 15:d6edd100d7fe 280 if(count2_2 >4) {
baraki 15:d6edd100d7fe 281 count2 = 0;
baraki 15:d6edd100d7fe 282 }
baraki 0:ce4f790399d9 283 }
baraki 15:d6edd100d7fe 284 else
baraki 15:d6edd100d7fe 285 down_cal2 = distance2;
baraki 0:ce4f790399d9 286
baraki 0:ce4f790399d9 287 //find DOWN distance
baraki 0:ce4f790399d9 288 char receiveData3[3] = {0};
baraki 2:ec53792aef80 289 i = 0;
baraki 0:ce4f790399d9 290 sensor.write(addresses[6], sendData, 1);
baraki 2:ec53792aef80 291 while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){
baraki 0:ce4f790399d9 292 i++;}
baraki 0:ce4f790399d9 293 int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
baraki 15:d6edd100d7fe 294 if(count > 50) { //calibration over
baraki 8:2ddeec5d8f84 295 //get allowableX and adjusted down_cal from IMU
baraki 8:2ddeec5d8f84 296 imu.get_angles();
baraki 8:2ddeec5d8f84 297 float pitch = imu.euler.pitch;
rkk 13:b22fd9c4fbb4 298 float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
rkk 13:b22fd9c4fbb4 299 float allowX = ALLOW_HEIGHT/cosp;
baraki 8:2ddeec5d8f84 300 float new_down_cal = ((float)down_cal)*cospi/cosp;
baraki 8:2ddeec5d8f84 301
baraki 8:2ddeec5d8f84 302 //use moving average to find deltaX
baraki 8:2ddeec5d8f84 303 int difference = abs(new_down_cal - distance3);
baraki 0:ce4f790399d9 304 differenceAvgSum = differenceAvgSum - moving_ave[count2];
baraki 0:ce4f790399d9 305 moving_ave[count2] = difference;
baraki 0:ce4f790399d9 306 differenceAvgSum = differenceAvgSum + difference;
baraki 0:ce4f790399d9 307 count2 = count2 + 1;
baraki 0:ce4f790399d9 308 int ave = (int)(differenceAvgSum/5);
baraki 7:660e8ddb231e 309
rkk 13:b22fd9c4fbb4 310 //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 311
baraki 3:a0ccaf565e8d 312 //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
baraki 7:660e8ddb231e 313 if(ave >= allowX) {
baraki 0:ce4f790399d9 314 pulses[6] = 5;
baraki 0:ce4f790399d9 315 intensity[6] = 7;
baraki 0:ce4f790399d9 316 } else {
rkk 5:d4ed744beea2 317 pulses[6] = 1;
baraki 0:ce4f790399d9 318 intensity[6] = 0;
baraki 0:ce4f790399d9 319 }
baraki 0:ce4f790399d9 320
baraki 0:ce4f790399d9 321 if(count2 >4) {
baraki 0:ce4f790399d9 322 count2 = 0;
baraki 0:ce4f790399d9 323 }
baraki 0:ce4f790399d9 324 } else {
baraki 8:2ddeec5d8f84 325
baraki 0:ce4f790399d9 326 down_cal = distance3;
baraki 8:2ddeec5d8f84 327 imu.get_angles();
baraki 8:2ddeec5d8f84 328 float pitch = imu.euler.pitch;
rkk 13:b22fd9c4fbb4 329 cospi = cos((pitch-DOWN_ANGLE) * PI/180.0);
baraki 0:ce4f790399d9 330 count = count+1;
baraki 0:ce4f790399d9 331 }
baraki 3:a0ccaf565e8d 332 //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",6,pulses[6],intensity[6]);
baraki 0:ce4f790399d9 333
baraki 1:5b1d88d69aa2 334 //pc.printf("about to send data\n");
baraki 0:ce4f790399d9 335 btData[0] = (pulses[0] << 5) | (intensity[0] << 2);
baraki 0:ce4f790399d9 336 btData[1] = (pulses[1] << 4) | (intensity[1] << 1);
baraki 0:ce4f790399d9 337 btData[2] = (pulses[2] << 3) | (intensity[2]);
baraki 0:ce4f790399d9 338 btData[3] = (pulses[3] << 2) | (intensity[3] >> 1);
baraki 0:ce4f790399d9 339 btData[4] = (intensity[3] << 7) | (pulses[4] << 1) | (intensity[4] >> 2);
baraki 0:ce4f790399d9 340 btData[5] = (intensity[4] << 6) | (0x3);
baraki 0:ce4f790399d9 341 btData[6] = (pulses[5] << 5) | (intensity[5] << 2);
baraki 0:ce4f790399d9 342 btData[7] = (pulses[6] << 5) | (intensity[6] << 2);
baraki 0:ce4f790399d9 343 btData[8] = '\0';
baraki 0:ce4f790399d9 344 for(int j=0;j<9;j++){
baraki 1:5b1d88d69aa2 345 if(bt.writeable())
baraki 1:5b1d88d69aa2 346 bt.putc(btData[j]);
baraki 1:5b1d88d69aa2 347 //wait(0.001);
baraki 0:ce4f790399d9 348 }
baraki 3:a0ccaf565e8d 349 wait(0.05);
baraki 1:5b1d88d69aa2 350 //pc.printf("finished sending data\n");
baraki 0:ce4f790399d9 351 //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9);
baraki 0:ce4f790399d9 352 }
baraki 0:ce4f790399d9 353 }