File content as of revision 0:59ae1ba97a94:

#include "mbed.h"
#include "Sabertooth.h"
#include "rtos.h"
#include "peixingv2.h"
#include "LidarLitev2.h"

Serial pc(USBTX, USBRX);
Serial device(p28, p27);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

DigitalIn SignalIn(p29);
DigitalOut SignalOut(p30);
LidarLitev2 Lidar(p9, p10);
PwmOut servo(p21);

AnalogIn s1 (p15);
AnalogIn s2 (p16);
AnalogIn s3 (p19);
AnalogIn s4 (p20);

#define k_5 12466.0
#define k_4 -23216.0
#define k_3 14974.0
#define k_2 -3585.0
#define k_1 19.0
#define k_0 96.0

int i;
float val1, dist1, total_dist1, ave_dist1;
float val2, dist2, total_dist2, ave_dist2;
bool x;

void read_sensors()
{
    dist1 = 0;
    total_dist1 = 0;
    ave_dist1 = 0;

    dist2 = 0;
    total_dist2 = 0;
    ave_dist2 = 0;

    for(i = 0; i < 50; i++) 
    {
        val1 = s1.read();
        dist1 = 0;
        dist1 += k_5*(val1*val1*val1*val1*val1);
        dist1 += k_4*(val1*val1*val1*val1);
        dist1 += k_3*(val1*val1*val1);
        dist1 += k_2*(val1*val1);
        dist1 += k_1*val1;
        dist1 += k_0;
        total_dist1 = total_dist1 + dist1;

        val2 = s2.read();
        dist2 = 0;
        dist2 += k_5*(val2*val2*val2*val2*val2);
        dist2 += k_4*(val2*val2*val2*val2);
        dist2 += k_3*(val2*val2*val2);
        dist2 += k_2*(val2*val2);
        dist2 += k_1*val2;
        dist2 += k_0;
        total_dist2 = total_dist2 + dist2;
    }
    ave_dist1 = total_dist1 / 50;
    ave_dist2 = total_dist2 / 50;
}

void thread1(void const *args)
{
    while(true)
    {
        read_sensors();
        int speed = 20;
        if(ave_dist1 < 30 || ave_dist2 < 30) SignalOut = 1;
        else SignalOut = 0;
        if(ave_dist1 < 30 && ave_dist2 >= 30) 
        {
            x = false;
            turnright(speed);
        }
        else if(ave_dist2 < 30 && ave_dist1 >= 30) 
        {
            x = false;
            turnleft(speed);
        }
        else if(ave_dist2 < 30 && ave_dist1 < 30) 
        {
            x = false;
            turnright(speed);
        }
        else x = true;
    }
}

int main()
{
    pc.baud(115200);
    device.baud(115200);
    front_sb.InitializeCom();
    back_sb.InitializeCom();
    Thread thread(thread1);
    int speed = 50;
    float distance;
    char c = 'k', a;
    Lidar.configure();
    servo.period_ms(20);
    while(true) 
    {
        if(x == true)
        {
            if(device.readable())
            {
                a = device.getc();
                if(a > 0 && a <= 100) speed = a;
                else if(a == 108)
                {
                    distance = 0;
                    for(int j = 0; j < 3; j++) distance = distance + Lidar.distance();
                    distance = distance / 3;
                    pc.printf("%d", int(distance));
                    device.putc(int(distance));
                }
                else if(a >= 109)
                {
                    servo.pulsewidth_us(1089 + (a - 109) * 7);
                    wait(0.01);
                }
                else c = a;
            }
            if(c == 'g') forward(speed);
            else if(c == 'h') backward(speed);
            else if(c == 'j') right(speed);
            else if(c == 'i') left(speed);
            else if(c == 'e') turnright(speed);
            else if(c == 'f') turnleft(speed);
            else stop();
        }
    }
}