Diff: main.cpp

Revision:

17:32b83c5787fc

Parent:

16:70fce771d828

Child:

19:01d091b38d61

diff -r 70fce771d828 -r 32b83c5787fc main.cpp
--- a/main.cpp	Sat Oct 17 19:55:27 2015 +0000
+++ b/main.cpp	Wed Nov 25 23:23:26 2015 +0000
@@ -2,115 +2,28 @@
 #include "MODSERIAL.h"
 #include <math.h>
 #include "BNO055.h"
-//for calculating allowable change in height for the down lidar
-#define DOWN_ANGLE -5.6
-#define ALLOW_HEIGHT 120
-
-//Up Delta
-#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
-//#define RANGE_03 1700
-//#define RANGE_04 2750
-//#define UP_MIN 600
-//#define UP_MAX 1300
-
-// Mid range settings
-#define RANGE_01 450
-#define RANGE_02 750
-#define RANGE_03 1150
-#define RANGE_04 1550
-
-// Short range settings
-//#define RANGE_01 400
-//#define RANGE_02 650
-//#define RANGE_03 1000
-//#define RANGE_04 1350
-
 
 #define PC_BAUD 9600
-#define BT_BAUD 115200
-#define TX_PIN p13
-#define RX_PIN p14
-#define SDA_PIN p9   //SDA pin on LPC1768
-#define SCL_PIN p10  //SCL pin on LPC1768
+//#define BT_BAUD 115200
+
+//#define TX_PIN p13
+//#define RX_PIN p14
 #define IMU_SDA p28
 #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 bt(TX_PIN, RX_PIN);
 MODSERIAL pc(USBTX,USBRX);
-AnalogIn potentiometer(p18); //doesn't exist yet
-
 
 //for calibrating IMU
 //for IMU1:
-//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};
-//for IMU2:
-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};
-
-
-//for encoder:
-//need to set pins to DigitalIn with internal pullup resistors
-//DigitalIn mySwitch(p21);
-//mySwitch.mode(PullUp);
+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};
 
 bool newline_detected = false;
 bool newline_sent = false;
 
-float potValue(){
-    //float pot = potentiometer.read();
-    float pot = ((float)((char)pc.getc()) - '0')/18.0;
-    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();
 }
@@ -140,9 +53,9 @@
 int main()
 {
     pc.baud(PC_BAUD);
-    bt.baud(BT_BAUD);
+//    bt.baud(BT_BAUD);
     pc.attach(&rxCallback, MODSERIAL::RxIrq);
-    bt.attach(&txCallback, MODSERIAL::TxIrq);
+//    bt.attach(&txCallback, MODSERIAL::TxIrq);
     
     //set up IMU
     imu.reset();
@@ -154,200 +67,22 @@
         imu.get_calib();
     }
     
-    sensor.frequency(100000);
-
     char sendData[1] = {0x00};
 
-    int addresses[7];
-    addresses[0] = 0x60; //0x60
-    addresses[1] = 0x64; //0x64
-    addresses[2] = 0x68; //middle
-    addresses[3] = 0x6C;
-    addresses[4] = 0x70;
-    addresses[5] = 0x80; //up
-    addresses[6] = 0x84; //down
-
-    uint8_t pulses[7] = {0};
-    uint8_t intensity[7] = {0};
-
-    char btData[12] = {'a','b','c','d','e','f','g','\n','\0'};
-
-    //calibrate down sensor
-    int down_cal = 0;
-    int down_cal2 = 0;
-    float cospi = 0; //"cosine of initial pitch"
-    
-    unsigned int i = 0;
-    int count = 0; //for calibration
-    int count2 = 0;//for averaging
-    int count2_2 = 0; //for averaging second down sensor
-    int differenceAvgSum = 0;
-    int differenceAvgSum2 = 0;
-    int moving_ave[5] = {0};
-    int moving_ave2[5] = {0};
-    while (1) {
-        for(int k=0; k<5; k++) {
-            char receiveData[3] = {0};
-            if(sensor.write(addresses[k], sendData, 1)){
-                //pc.printf("writing to sensor %d failed\n", k);
-                }
-            //write ---> 0 on success, 1 on failure
-            i = 0;
-            while(sensor.read(addresses[k], receiveData, 3) && i < 10) {
-                i++;
-                //pc.printf("reading from sensor %d failed\n",k);
-                }
-                //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;
-                }
-                for(int z=0;z<4;z++)
-                    pc.printf("angle: %f radius %d is %f ",angle[k],z,radius[z]);
-                pc.printf("\n");
-                if(distance > 0 && distance < radius[0]) {
-                    pulses[k] = 5;
-                    intensity[k] = 7;
-                } else if(distance >= radius[0] && distance < radius[1]) {
-                    pulses[k] = 4;
-                    intensity[k] = 6;
-                } else if(distance >= radius[1] && distance < radius[2]) {
-                    pulses[k] = 3;
-                    intensity[k] = 5;
-                } else if(distance >= radius[2] && distance < radius[3]) {
-                    pulses[k] = 2;
-                    intensity[k] = 2;
-                } else if(distance >= radius[3]) {
-                    pulses[k] = 1;
-                    intensity[k] = 0;
-                }
-            //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",k,pulses[k],intensity[k]);
-        }
-
-        //find UP distance
-//        char receiveData2[3] = {0};
-//        sensor.write(addresses[5], sendData, 1);
-//        i = 0;
-//        while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
-//            i++;}
-//        int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
-//        if(distance2 >= UP_MIN && distance2 < UP_MAX) {
-//            pulses[5] = 5; ///TODO WAS: 5
-//            intensity[5] = 7;   ///TODO: 7
-//        } else {
-//            pulses[5] = 1;
-//            intensity[5] = 0;
-//        }
+//    char btData[12] = {'a','b','c','d','e','f','g','\n','\0'};
 
-        //NEW down sensor:
-        char receiveData2[3] = {0};
-        sensor.write(addresses[5], sendData, 1);
-        i = 0;
-        while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
-            i++;}
-        int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
-        if(count > 50) { //calibration over
-            //get allowableX and adjusted down_cal from IMU
-            imu.get_angles();
-            float pitch = imu.euler.pitch;
-            float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
-            float allowX =  ALLOW_HEIGHT/cosp;
-            float new_down_cal = ((float)down_cal2)*cospi/cosp;
-            
-            //use moving average to find deltaX
-            int difference = abs(new_down_cal - distance2);
-            differenceAvgSum2 = differenceAvgSum2 - moving_ave2[count2_2];
-            moving_ave2[count2_2] = difference;
-            differenceAvgSum2 = differenceAvgSum2 + difference;
-            count2_2 = count2_2 + 1;
-            int ave = (int)(differenceAvgSum2/5);
-            
-            //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);
-            
-            //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
-            if(ave >= allowX) {
-                pulses[5] = 5;
-                intensity[5] = 7;
-            } else {
-                pulses[5] = 1; 
-                intensity[5] = 0;
-            }
-
-            if(count2_2 >4) {
-                count2 = 0;
-            }
-        }
-        else
-            down_cal2 = distance2;
-
-        //find DOWN distance
-        char receiveData3[3] = {0};
-        i = 0;
-        sensor.write(addresses[6], sendData, 1);
-        while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){
-            i++;}
-        int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
-        if(count > 50) { //calibration over
-            //get allowableX and adjusted down_cal from IMU
-            imu.get_angles();
-            float pitch = imu.euler.pitch;
-            float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
-            float allowX =  ALLOW_HEIGHT/cosp;
-            float new_down_cal = ((float)down_cal)*cospi/cosp;
-            
-            //use moving average to find deltaX
-            int difference = abs(new_down_cal - distance3);
-            differenceAvgSum = differenceAvgSum - moving_ave[count2];
-            moving_ave[count2] = difference;
-            differenceAvgSum = differenceAvgSum + difference;
-            count2 = count2 + 1;
-            int ave = (int)(differenceAvgSum/5);
-            
-            //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);
-            
-            //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
-            if(ave >= allowX) {
-                pulses[6] = 5;
-                intensity[6] = 7;
-            } else {
-                pulses[6] = 1; 
-                intensity[6] = 0;
-            }
-
-            if(count2 >4) {
-                count2 = 0;
-            }
-        } else {
-            
-            down_cal = distance3;
-            imu.get_angles();
-            float pitch = imu.euler.pitch;
-            cospi = cos((pitch-DOWN_ANGLE) * PI/180.0);
-            count = count+1;
-        }
-        //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",6,pulses[6],intensity[6]);
-
-        //pc.printf("about to send data\n");
-        btData[0] = (pulses[0] << 5) | (intensity[0] << 2);
-        btData[1] = (pulses[1] << 4) | (intensity[1] << 1);
-        btData[2] = (pulses[2] << 3) | (intensity[2]);
-        btData[3] = (pulses[3] << 2) | (intensity[3] >> 1);
-        btData[4] = (intensity[3] << 7) | (pulses[4] << 1) | (intensity[4] >> 2);
-        btData[5] = (intensity[4] << 6) | (0x3);
-        btData[6] = (pulses[5] << 5) | (intensity[5] << 2);
-        btData[7] = (pulses[6] << 5) | (intensity[6] << 2);
-        btData[8] = '\0';
-        for(int j=0;j<9;j++){
-            if(bt.writeable())
-                bt.putc(btData[j]);
-            //wait(0.001);
-        }
-        wait(0.05);
+    while (1) {
+        imu.get_angles();
+        float pitch = imu.euler.pitch;
+        pc.printf("%f\n",pitch);
+//        pc.printf("about to send data\n");
+//        for(int j=0;j<9;j++){
+//            btData[j] = '\0';
+//            if(bt.writeable())
+//                bt.putc(btData[j]);
+//            //wait(0.001);
+//        }
+        wait(0.5);
         //pc.printf("finished sending data\n");
-        //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9);
     }
 }