File content as of revision 0:7a43aff338da:

#include "mbed.h"
#include "Sabertooth.h"
#include "peixing.h"

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

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;

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

int main() 
{
    pc.baud(115200);
    device.baud(115200);
    front_sb.InitializeCom();
    back_sb.InitializeCom();
    while(1)
    {
        read_sensors();
        pc.putc(device.getc());
        if(ave_dist1 >= 20 && ave_dist2 >= 20)
        {
            char c;
            /*while(device.readable() == false) 
            {
                read_sensors();
                if(ave_dist1 < 20 || ave_dist2 < 20) break;
            }
            if(ave_dist1 >= 20 && ave_dist2 >= 20 && device.readable()) c = device.getc();*/
            if(device.readable() == true) c = device.getc();
            else c = 'p';
            if(c == 'w') forward();
            else if(c == 's') backward();
            else if(c == 'd') right();
            else if(c == 'a') left();
            else if(c == 'l') turnright();
            else if(c == 'k') turnleft();
            else stop();
        }
        else if(ave_dist1 < 20 && ave_dist2 >= 20)
        {
            while(ave_dist1 < 20 && ave_dist2 >= 20)
            {
                read_sensors();
                front_sb.SetSpeedMotorA(-30);
                front_sb.SetSpeedMotorB(20);
                back_sb.SetSpeedMotorA(20);
                back_sb.SetSpeedMotorB(-30);
            }
            stop();
        }
        else if(ave_dist2 < 20 && ave_dist1 >= 20)
        {
            while(ave_dist2 < 20 && ave_dist1 >= 20)
            {
                read_sensors();
                front_sb.SetSpeedMotorA(20);
                front_sb.SetSpeedMotorB(-30);
                back_sb.SetSpeedMotorA(-30);
                back_sb.SetSpeedMotorB(20);
            }
            stop();
        }
        else if(ave_dist2 < 20 && ave_dist1 < 20)
        {
            while(ave_dist2 < 20 && ave_dist1 < 20)
            {
                read_sensors();
                turnright();
            }
            stop();
        }
        else stop();
    }  
}