Diff: main.cpp

Revision:

14:8620dceb3ba2

Parent:

13:b22fd9c4fbb4

Child:

15:d6edd100d7fe

diff -r b22fd9c4fbb4 -r 8620dceb3ba2 main.cpp
--- 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;
                 }