blue mbed code for the BNO055 imu from adafruit
Dependencies: BNO055 MODSERIAL mbed
Fork of bmbed_lidar_belt by
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?
User | Revision | Line number | New 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 | } |