File content as of revision 8:2ddeec5d8f84:

#include "mbed.h"
#include "MODSERIAL.h"
#include <math.h>
#include "BNO055.h"

#define PC_BAUD 9600
#define BT_BAUD 9600
#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

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);

//for calculating allowable change in height for the down lidar
float allowHeight = 120; //120 mm
float downAngle = -7.0;
//for calibrating IMU
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 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;

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;
    }
}

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;
    float cospi = 0; //"cosine of initial pitch"
    
    unsigned int i = 0;
    int count = 0; //for calibration
    int count2 = 0;//for averaging
    int differenceAvgSum = 0;
    int moving_ave[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(distance == 0){
                  pulses[k] = 1;
                  intensity[k] = 0;
                }
                if(distance > 0 && distance < 650) {
                    pulses[k] = 5;
                    intensity[k] = 7;
                } else if(distance >= 650 && distance < 900) {
                    pulses[k] = 4;
                    intensity[k] = 6;
                } else if(distance >= 900 && distance < 1350) {
                    pulses[k] = 3;
                    intensity[k] = 5;
                } else if(distance >= 1350 && distance < 1850) {
                    pulses[k] = 2;
                    intensity[k] = 2;
                } else if(distance >= 1800) {
                    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 >= 500 && distance2 < 1000) {
            pulses[5] = 5;
            intensity[5] = 7;
        } else {
            pulses[5] = 1;
            intensity[5] = 0;
        }

        //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 > 100) { //calibration over
            //get allowableX and adjusted down_cal from IMU
            imu.get_angles();
            float pitch = imu.euler.pitch;
            float cosp = cos((pitch-downAngle) * PI/180.0);
            float allowX =  allowHeight/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-downAngle,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-downAngle) * 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);
        //pc.printf("finished sending data\n");
        //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9);
    }
}