File content as of revision 0:044f44184170:

#include "mbed.h"
#include "XNucleo53L0A1.h" //Lidar Distance Sensor: 3.3V VIN, GND GND, P13 SDA, P14 SCL, P26 SHDN
#include "Servo.h"
#include "rtos.h"
#include <stdio.h>

BusOut myled(LED1,LED2,LED3,LED4);
Serial blue(p13,p14); //bluetooth UART

Servo myservo(p23); //Servo connected to PWM 23

float p;
PwmOut ledR(p26); //RGB Red
PwmOut ledG(p25); //RGB Green
PwmOut ledB(p24); //RGB Blue

PwmOut SpeedL(p21); //Speed of Left motor
PwmOut SpeedR(p22); //Speed of Right motor
DigitalOut FwdL(p15); //Forward Left motor
DigitalOut FwdR(p16); //Forward Right motor
DigitalOut BckL(p19); //Backward Left motor
DigitalOut BckR(p18); //Backward Right motor

/* I2C LIDAR sensor */
#define VL53L0_I2C_SDA   p28
#define VL53L0_I2C_SCL   p27
DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
static XNucleo53L0A1 *board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);; // LIDAR singleton obj instantiate
DigitalOut shdn(p29);
int status;
uint32_t sensorData;
volatile int distance;
int DistFlag; //used to determine if collision would have occured

Thread t1;
Thread t2;

void sensor()
{
    shdn = 0; //must reset sensor for an mbed reset to work
    wait(0.1);
    shdn = 1;
    wait(0.1);
    // init the 53L0A1 board with default values
    status = board->init_board();
    while (status) {
        pc.printf("Failed to init board! \r\n");
        status = board->init_board();
    }
    pc.printf("Board Init. Beginning Loop \r\n");
    //loop taking and printing distance
    while (1) {
        status = board->sensor_centre->get_distance(&sensorData);
        distance = sensorData;
        if (status == VL53L0X_ERROR_NONE) {
            if (distance <= 400) DistFlag = 1;
            pc.printf("D=%ld mm // DistFlag = %d\r\n", distance, DistFlag);
        }
        Thread::wait(10);
    }//end while
} // end sensor

void motor()
{
    char bnum=0;
    char bhit=0;
    while (1) {
        if (DistFlag == 1) {
            ledG = 0;
            ledR = 1;
            SpeedL = 0.5;
            SpeedR = 0.5;
            FwdL = 0;
            FwdR = 0;
        } else {
            ledG = 1;
            if (blue.readable()) {
                if (blue.getc()=='!') {
                    if (blue.getc()=='B') { //button data packet
                        bnum = blue.getc(); //button number
                        bhit = blue.getc(); //1=hit, 0=release
                        if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                            myled = bnum - '0'; //current button number will appear on LEDs
                            switch (bnum) {
                                case '1': //number button 1: Servo Sweep
                                    if (bhit=='1') {
                                        for(float p=0; p<1.0; p += 0.1) {
                                            myservo = p;
                                            wait(0.2);
                                        }
                                    } else {
                                        myservo = 0.5;
                                    }
                                    break;
                                case '2': //number button 2
                                    if (bhit=='1') {
                                        //add hit code here
                                        DistFlag = 0;
                                    } else {
                                        //add release code here
                                    }
                                    break;
                                case '3': //number button 3
                                    if (bhit=='1') {
                                        //add hit code here
                                    } else {
                                        //add release code here
                                    }
                                    break;
                                case '4': //number button 4
                                    if (bhit=='1') {
                                        //add hit code here
                                    } else {
                                        //add release code here
                                    }
                                    break;
                                case '5': //button 5 up arrow
                                    //pc.printf("Pressed F\n");
                                    if (bhit=='1') {
                                        SpeedL = 1;
                                        SpeedR = 1;
                                        FwdL = 1;
                                        FwdR = 1;
                                    } else {
                                        for (p = SpeedL; p > 0; p -= 0.1) {
                                            SpeedL = p;
                                            SpeedR = p;
                                        }
                                        FwdL = 0;
                                        FwdR = 0;
                                    }
                                    break;
                                case '6': //button 6 down arrow
                                    if (bhit=='1') {
                                        SpeedL = 0.5;
                                        SpeedR = 0.5;
                                        BckL = 1;
                                        BckR = 1;
                                    } else {
                                        for (p = SpeedL; p > 0; p -= 0.1) {
                                            SpeedL = p;
                                            SpeedR = p;
                                        }
                                        BckL = 0;
                                        BckR = 0;
                                    }
                                    break;
                                case '7': //button 7 left arrow
                                    if (bhit=='1') {
                                        SpeedL = 0.5;
                                        SpeedR = 0.5;
                                        BckL = 1;
                                        FwdR = 1;
                                    } else {
                                        for (p = SpeedL; p > 0; p -= 0.1) {
                                            SpeedL = p;
                                            SpeedR = p;
                                        }
                                        BckL = 0;
                                        FwdR = 0;
                                    }
                                    break;
                                case '8': //button 8 right arrow
                                    if (bhit=='1') {
                                        SpeedL = 0.5;
                                        SpeedR = 0.5;
                                        FwdL = 1;
                                        BckR = 1;
                                    } else {
                                        for (p = SpeedL; p > 0; p -= 0.1) {
                                            SpeedL = p;
                                            SpeedR = p;
                                        }
                                        FwdL = 0;
                                        BckR = 0;
                                    }
                            }
                        }
                    }
                }
            } // end readable
        } // end else
        Thread::wait(100);
    } //end while
} // end Motor

int main()
{
    DistFlag = 0;
    for (p = 0; p < 1; p += 0.1) {
        ledB = p;
        wait(0.2);
    }
    wait(0.05);
    ledB = 0;
    t1.start(sensor);
    t2.start(motor);
    while (1) {
        if (DistFlag == 1) {
            while (distance <= 400) {
                SpeedL = 0.2;
                SpeedR = 0.2;
                BckL = 1;
                BckR = 1;
                Thread::wait(10); //Reupdate Distance
            }
            BckL = 0;
            BckR = 0;
            ledR = 0;
            DistFlag = 0;
        }
        Thread::wait(500);
    }
}