File content as of revision 0:1676cb5a5d33:

#include "mbed.h"
#include "Motor.h"
#include "rtos.h"
#include "ultrasonic.h"

Serial bluemod(p13,p14); //change blt to this
PwmOut left_led(LED1);
PwmOut right_led(LED4);
PwmOut forward_led(LED2);
PwmOut reverse_led(LED3);

DigitalOut trig(p6);
DigitalOut echo(p7);

Serial pc(USBTX, USBRX);

Motor m1(p26, p25, p24); // left motor : pwm, fwd, rev
Motor m2(p21, p22, p23); // right motor : pwm, fwd, rev

bool lidarFlag1 = 0;
int flag_distance;
int g_distance;
void dist(int distance) {
    g_distance = distance;
    if(g_distance < flag_distance) lidarFlag1 = 1;
    pc.printf("Distance %d mm\r\n", g_distance);
}
ultrasonic mu(p6, p7, .1, 1, &dist);

//C union can convert 4 chars to a float - puts them in same location in memory
//trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float
union f_or_char {
    float f;
    char  c[4];
};

void calcSpeed(float x, float y, float th) {
    float left;
    float right;
    if (x > th) { //moving forward
        left = x;
        right = x;
    } else if (x < -th) { //reverse
        left = x;
        right = x;
    } else {
        left = 0.0;
        right = 0.0;
    }
    
    flag_distance = left * 400;
    if (flag_distance < 250) flag_distance = 250;
    
    //turning
    if (y > th) {//turning left
        if (x > 0.2) {
            right += y/2;
            left -= y/2;
        } else {
            right -= y/2;
            left += y/2;
        }     
    } else if (y < -th) { //turning right
        if (x > 0.2) {
            left -= y/2;
            right += y/2;
        } else {
            left += y/2;
            right -= y/2;
        }
    }
    
    //check that speed is within the range [-1,1]
    if (left > 1.0) {
        left = 1.0;
    } else if (left < -1.0) {
        left = -1.0;
    }
    //check that speed is within the range [-1,1]
    if (right > 1.0) {
        right = 1.0;
    } else if (right < -1.0) {
        right = -1.0;
    }
    
    //set speed
    m1.speed(left);
    m2.speed(right);
}

void speed()
{   
    char bchecksum=0;
    char temp=0;
    
    union f_or_char x,y,z;
    while(1) {
        bchecksum=0;
        if (bluemod.getc()=='!') {
            if (bluemod.getc()=='A') { //Accelerometer data packet
                for (int i=0; i<4; i++) {
                    temp = bluemod.getc();
                    x.c[i] = temp;
                    bchecksum = bchecksum + temp;
                }
                for (int i=0; i<4; i++) {
                    temp = bluemod.getc();
                    y.c[i] = temp;
                    bchecksum = bchecksum + temp;
                }
                for (int i=0; i<4; i++) {
                    temp = bluemod.getc();
                    z.c[i] = temp;
                    bchecksum = bchecksum + temp;
                }
                if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK?
                    //printf("X = %f  Y = %f  Z = %f\n\r",x.f, y.f, z.f);
                    right_led = left_led = forward_led = reverse_led = 0.0;
                    if (x.f<=0.0) right_led = -x.f/10.0;//Scale to 0.0 to 1.0 for PWM so /10.0
                    else left_led = x.f/10.0;
                    if (y.f<=0.0) forward_led = -y.f/10.0;
                    else reverse_led = y.f/10.0; 
                    
                    if(!lidarFlag1) calcSpeed(x.f, y.f, 0.2f); 
                } 
            }
        } 
    } 
}

int main()
{  
    Thread thread1;
    thread1.start(speed);
    mu.startUpdates();
    while(1) {
        mu.checkDistance();
        if(lidarFlag1==1) {
            while(g_distance < 250) {
                mu.checkDistance();
                m1.speed(-0.2);
                m2.speed(-0.2);
            }
            m1.speed(0.0);
            m2.speed(0.0);
            lidarFlag1 = 0;
        }
    } 
}