blue mbed code for the BNO055 imu from adafruit
Dependencies: BNO055 MODSERIAL mbed
Fork of bmbed_lidar_belt by
Diff: main.cpp
- Revision:
- 14:8620dceb3ba2
- Parent:
- 13:b22fd9c4fbb4
- Child:
- 15:d6edd100d7fe
--- a/main.cpp Fri Oct 16 20:10:10 2015 +0000 +++ b/main.cpp Sat Oct 17 19:02:41 2015 +0000 @@ -10,6 +10,16 @@ #define UP_MIN 500 #define UP_MAX 1100 +float MIN_MIN_A=200.0; //min side range +float MAX_MIN_A=500.0; //min side range +float MIN_MAX_A=400.0; //max side range +float MAX_MAX_A=1200.0; //max side range + +float MIN_MIN_B=400.0; //min front range +float MAX_MIN_B=1000.0; //min front range +float MIN_MAX_B=1200.0; //max front range +float MAX_MAX_B=2750.0; //max front range + //Initial LONG Range Settings //#define RANGE_01 750 //#define RANGE_02 1100 @@ -41,11 +51,14 @@ #define IMU_SCL p27 #define PI 3.14159265 +float angle[5] = {PI, 3*PI/4, PI/2, PI/4, 0}; //relates lidar number to its angle in radians + BNO055 imu(p28,p27); I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1 MODSERIAL bt(TX_PIN, RX_PIN); MODSERIAL pc(USBTX,USBRX); +AnalogIn potentiometer(p18); //doesn't exist yet //for calibrating IMU @@ -63,6 +76,40 @@ bool newline_detected = false; bool newline_sent = false; +float potValue(){ + float pot = potentiometer.read(); + if(pot <= 0.5) + return 2*pot; + else + return 2*(1-pot); //if pot = 1, return 0. if pot = 0.5, return 1 +} + +void angleToRadius(float* radius,float angle){ + //a is horizontal semi-axis + //b is vertical semi-axis + //scale between MIN_A, MAX_A, MIN_B, MAX_B + //using pot value + float pot = potValue(); + //define new min_a scaled between MIN_MIN_A and MAX_MIN_A + float min_a = MIN_MIN_A + (MAX_MIN_A - MIN_MIN_A)*pot; + //define new max_a scaled between MIN_MAX_A and MAX_MAX_A + float max_a = MIN_MAX_A + (MAX_MAX_A - MIN_MAX_A)*pot; + //define new min_b between MIN_MIN_B and MAX_MIN_B + float min_b = MIN_MIN_B + (MAX_MIN_B - MIN_MIN_B)*pot; + //define new max_b scaled between MIN_MAX_B and MAX_MAX_B + float max_b = MIN_MAX_B + (MAX_MAX_B - MIN_MAX_B)*pot; + + float scale[4] = {0, 0.3, 0.6,1.0}; + for(int i=0;i<4;i++){ + //scale a and be to be somewhere between the min and max based on i + // i = 0 -- min + // i = 5 -- max + float a = min_a + (max_a - min_a)*scale[i]; + float b = min_b + (max_b - min_b)*scale[i]; + radius[i] = a*b/sqrt(pow(a,2)*pow(sin(angle),2) + pow(b,2)*pow(cos(angle),2)); + } +} + void setCal(){ imu.write_calibration_data(); } @@ -148,23 +195,25 @@ //while(!twi_master_transfer(addresses[k], sendData, 1, TWI_ISSUE_STOP)){;} //while(!twi_master_transfer(addresses[k] + 1, receiveData, 3, TWI_ISSUE_STOP)){;} int distance = ((int)receiveData[0]<<8 )+ (int)receiveData[1]; + float radius[4] = {0}; + angleToRadius(radius, angle[k]); if(distance == 0){ pulses[k] = 1; intensity[k] = 0; } - if(distance > 0 && distance < RANGE_01) { + if(distance > 0 && distance < radius[0]) { pulses[k] = 5; intensity[k] = 7; - } else if(distance >= RANGE_01 && distance < RANGE_02) { + } else if(distance >= radius[0] && distance < radius[1]) { pulses[k] = 4; intensity[k] = 6; - } else if(distance >= RANGE_02 && distance < RANGE_03) { + } else if(distance >= radius[1] && distance < radius[2]) { pulses[k] = 3; intensity[k] = 5; - } else if(distance >= RANGE_03 && distance < RANGE_04) { + } else if(distance >= radius[2] && distance < radius[3]) { pulses[k] = 2; intensity[k] = 2; - } else if(distance >= RANGE_04) { + } else if(distance >= radius[3]) { pulses[k] = 1; intensity[k] = 0; }