File content as of revision 2:7da645fba3eb:

#include "mbed.h"
#include "MODSERIAL.h"
#include <math.h>
#include "BNO055.h"
//for calculating allowable change in height for the down lidar
#define DOWN_ANGLE -3.8 //#1
//#define DOWN_ANGLE -8.3 //#2
//#define DOWN_ANGLE -9.0 //#3
#define ALLOW_HEIGHT 120

//Up Delta
#define UP_MIN 500
#define UP_MAX 1100

//for scaling braille output
float MAX_MAX_braille = 3000.0; //maximum max distance braille can see
float MIN_MAX_braille = 1000.0; //minimum max distance braile can see
float MIN_braille = 210.0; //the minimum that braille can see is 200 (limited by LIDAR)
#define BRAILLE_STEPS 7 //only 7 right now... ideally 8 but I messed up braille code a bit
bool brailleMode = 1; //1 if brailleMode; 0 if vibrator mode
char brailleChar[12] = {0};

float PMIN_EMIN_A=300.0; //min side range
float PMAX_EMIN_A=700.0; //min side range
float PMIN_EMAX_A=900.0; //max side range
float PMAX_EMAX_A=1600.0; //max side range

float PMIN_EMIN_B=400.0; //min front range
float PMAX_EMIN_B=1000.0; //min front range
float PMIN_EMAX_B=1200.0; //max front range
float PMAX_EMAX_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 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 pc(USBTX,USBRX);
AnalogIn potentiometer(p20);


//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] = {239, 255, 27, 0, 5, 0, 76, 255, 230, 0, 115, 255, 255, 255, 254, 255, 1, 0, 232, 3, 106, 3}; //#2
char cal_vals[22] = {255, 255, 2, 0, 18, 0, 58, 255, 236, 0, 156, 255, 1, 0, 255, 255, 255, 255, 232, 3, 43, 3}; //#1
//char cal_vals[22] = {237, 255, 18, 0, 24, 0, 144, 255, 232, 0, 54, 255, 0, 0, 254, 255, 0, 0, 232, 3, 87, 3}; //#3

//for encoder:
//need to set pins to DigitalIn with internal pullup resistors
//DigitalIn mySwitch(p21);
//mySwitch.mode(PullUp);

bool newline_detected = false;
bool newline_sent = false;

// lowest at 0 and 1; highest at 0.5
float potValue(){
    float pot = potentiometer.read();
    //pc.printf("pot: %f ",pot);
    //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 PMIN_EMIN_A and PMAX_EMIN_A
    float min_a = PMIN_EMIN_A + (PMAX_EMIN_A - PMIN_EMIN_A)*pot;
    //define new max_a scaled between PMIN_EMAX_A and PMAX_EMAX_A
    float max_a = PMIN_EMAX_A + (PMAX_EMAX_A - PMIN_EMAX_A)*pot;
    //define new min_b between PMIN_EMIN_B and PMAX_EMIN_B
    float min_b = PMIN_EMIN_B + (PMAX_EMIN_B - PMIN_EMIN_B)*pot;
    //define new max_b scaled between PMIN_EMAX_B and PMAX_EMAX_B
    float max_b = PMIN_EMAX_B + (PMAX_EMAX_B - PMIN_EMAX_B)*pot;
    
    float scale[4] = {0, 0.25, 0.6,1.0};
    //0 is min range
    //higher is higher range
    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();
}

// Called everytime a new character goes into
// the RX buffer. Test that character for \n
// Note, rxGetLastChar() gets the last char that
// we received but it does NOT remove it from
// the RX buffer.
void rxCallback(MODSERIAL_IRQ_INFO *q)
{
    MODSERIAL *serial = q->serial;
    if ( serial->rxGetLastChar() == '\n') {
        newline_detected = true;
    }

}

void txCallback(MODSERIAL_IRQ_INFO *q)
{
    MODSERIAL *serial = q->serial;
    if ( serial->txGetLastChar() == '\0') {
        newline_sent = true;
    }
}

void dist_to_str(float dist, char* braille){
    // the low numbers are farther away
    //braille must be at least 9 chars long
    braille[0] = '1';
    braille[8] = '\0';
    float max_braille = MIN_MAX_braille + (MAX_MAX_braille - MIN_MAX_braille)*potValue();
    float step_size = (max_braille - MIN_braille)/BRAILLE_STEPS;
    for(int i = 1;i<8;i++){
        if(dist > (MIN_braille + step_size*(BRAILLE_STEPS - i))) //if dist is greater than a certain step size, that step is clear
            braille[i] = '0';
        else
            braille[i] = '1';
    }
}

char str_to_bin(char *str){
    char brailleCharacter = 0;
    for(int i=0;i<8;i++){
        int val = str[i] - '0'; //0 if 0, 1 if 1
        if(val){
           int add = (int) pow((float)2.0,(float)i);
           brailleCharacter += add;
        }
    }
    //pc.printf("sending: %d\n",(int)brailleCharacter);
    return brailleCharacter;
}

char dist_to_bin(float dist){
    char braille[9] = {0};
    dist_to_str(dist,braille);
    //pc.printf("sending: %s\t",braille);
    return str_to_bin(braille);
}

//convert old column format to new and improved format
//old format:
//++++++++++  0   1
//0011223344  1   2
//0011223344  2   4
//0011223344  3   8
//0011223344  4   16
//0011223344  5   32
//0011223344  6   64
//0011223344  7   128

//new format
//0123456789

//++++++++++  0   1
//--222222--  1   2
//--222222--  2   4
//--222222--  3   8
//1111223333  4   16
//1111223333  5   32
//0000224444  6   64
//0000224444  7   128
void convertBrailleChar(char *brailleData){
    char lidar0 = brailleData[0] >> 4; //shift lidar0,1,3,4 data over by 4 bits (only loooking at nearest 4 data bits)
    char lidar1 = brailleData[2] >> 4;
    char lidar2 = brailleData[4];
    char lidar3 = brailleData[6] >> 4;
    char lidar4 = brailleData[8] >> 4;
    
    //clear brailleData
    for(int i=0;i<10;i++)
        if(i != 4 && i != 5)
            brailleData[i] = 1;
    
    for(int i=0;i<4;i++){ //add lidar0 and lidar1 data to first 4 columns
        int bit0 = (lidar0 >> i) & 0x01; //we want the 1st bit 1st, 4th bit 4th. Then bit mask to show only the 0x01 bit.
        brailleData[i] += 64*bit0 + 128*bit0; //add it to bits 6 and 7
        
        int bit1 = (lidar1 >> i) & 0x01; //lidar 1 (rows 2, 3; columns 0-3
        brailleData[i] += 32*bit1 + 16*bit1; 
        
        int j = 9-i;
        int bit4 = (lidar4 >> i) & 0x01; //lidar 4 (rows 0, 1; columns 6-9)
        brailleData[j] += 64*bit4 + 128*bit4;
        
        int bit3 = (lidar3 >> i) & 0x01; //lidar 3 (rows 2, 3; columns 6-9)
        brailleData[j] += 32*bit3 + 16*bit3;
        
        if(i == 2 || i == 3){ //add lidar 2 data to top 4 rows of columns 2,3 and 6,7
            uint8_t bit5 = ((lidar2 >> 1) << 5);// 0x00001110; //we are only interested in the smallest 3 bits (not including the LSB)
            bit5 = bit5/16;
            pc.printf("lidar2: %d, brailleData: %d, bit5: %d\n",lidar2,brailleData[i],bit5);
            brailleData[i] += bit5; //no need to scale bits
            brailleData[j] += bit5;
        }
    }
   // brailleData[4] += (lidar2 & 0x11111110);
   // brailleData[5] += (lidar2 & 0x11111110);
    
}

int main()
{
    pc.baud(PC_BAUD);
    bt.baud(BT_BAUD);
    pc.attach(&rxCallback, MODSERIAL::RxIrq);
    bt.attach(&txCallback, MODSERIAL::TxIrq);
    
    //set up IMU
    imu.reset();
    imu.setmode(OPERATION_MODE_NDOF);
    setCal();
    imu.get_calib();
    while (imu.calib == 0)
    {
        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];
            if(brailleMode) {
                brailleChar[2*k] = dist_to_bin(((float)distance));
                brailleChar[2*k+1] = brailleChar[2*k];
                //if(k == 4)
                //    pc.printf("\n");
            } else {
                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;
//        }

        imu.get_angles();
        float pitch = imu.euler.pitch;
        //pc.printf("pitch: %f\n",pitch);

        //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
//            pc.printf("measuring up\n");
//            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{
//            pulses[5] = 1;
//            intensity[5] = 0;
//            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
            float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
            float allowX =  ALLOW_HEIGHT/cosp;
            float new_down_cal = ((float)down_cal)*cospi/cosp;
            
            float new_down_cal2 = ((float)down_cal2)*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;
            int ave = (int)(differenceAvgSum/5);
            
            int difference2 = abs(new_down_cal2 - distance2);
            differenceAvgSum2 = differenceAvgSum2 - moving_ave2[count2];
            moving_ave2[count2] = difference2;
            differenceAvgSum2 = differenceAvgSum2 + difference2;
            count2 = count2 + 1;
            int ave2 = (int)(differenceAvgSum2/5);
            
            count2 = count2 + 1;
            
            //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(!brailleMode) {
                if(ave >= allowX) {
                    pulses[6] = 5;
                    intensity[6] = 7;
                } else {
                    pulses[6] = 1;
                    intensity[6] = 0;
                }

                if(ave2 >= allowX) {
                    pulses[5] = 5;
                    intensity[5] = 7;
                } else {
                    pulses[5] = 1;
                    intensity[5] = 0;
                }
            }
            
            if(brailleMode){
                if(ave >= allowX || ave2 >= allowX){
                    brailleChar[4] = dist_to_bin(10.0);
                    brailleChar[5] = brailleChar[4];
                }
            }

            if(count2 >4) {
                count2 = 0;
            }
        } else {
            down_cal2 = distance2;
            down_cal = distance3;
            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");
        if(!brailleMode){
            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);
            }
        }
        else{
            convertBrailleChar(brailleChar);
            
            if(bt.writeable()){
//                brailleChar[0] = 1;
//                brailleChar[1] = 1+2;
//                brailleChar[2] = 1+2+4;
//                brailleChar[3] = 1+2+4+8;
//                brailleChar[4] = 1+2+4+8+16;
//                brailleChar[5] = 1+2+4+8+16+32;
//                brailleChar[6] = 1+2+4+8+16+32+64;
//                brailleChar[7] = 1+2+4+16+32+64+128;
//                brailleChar[8] = 1+2+4;
//                brailleChar[9] = 1+2;
                for(int i=0;i<10;i++){
                    bt.putc(brailleChar[i]);
                }
                while(!bt.writeable()){}
                bt.putc('\0');
            }
            wait(0.2);
        }

        wait(0.05);
        //pc.printf("finished sending data\n");
        //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9);
    }
}