File content as of revision 1:2859bfed20b4:

//Developer Kang, Lee

#include "mbed.h"
#include "Ultrasonic.h"

AnalogIn Sharp(p20);
Ultrasonic F_sonic_R(p11,p11);
Ultrasonic F_sonic_L(p12,p12);
Ultrasonic F_sonic_F(p8,p8);

PwmOut RMotor_Front(p21);
PwmOut LMotor_Front(p23);
PwmOut RMotor_Back(p22);
PwmOut LMotor_Back(p24);
DigitalOut RMotor_EN(p5);
DigitalOut LMotor_EN(p6);
Timeout motortime;
Timeout backmotortime;
Timeout Rightmotortime;
Timeout Leftmotortime;
float a,b =0;
float length;
void Front()
{
    RMotor_EN = 1;
    LMotor_EN = 1;
    RMotor_Back.pulsewidth(0);
    LMotor_Back.pulsewidth(0);
    RMotor_Front.pulsewidth(0.05);
    LMotor_Front.pulsewidth(0.05);
}

void Turn_R()
{
    RMotor_EN = 1;
    LMotor_EN =1;
    LMotor_Front.pulsewidth(0);
    RMotor_Back.pulsewidth(0);
    LMotor_Back.pulsewidth(0.1);
    RMotor_Front.pulsewidth(0.1);
}

void Turn_L()
{
    LMotor_EN = 1;
    RMotor_EN = 1;
    LMotor_Back.pulsewidth(0);
    RMotor_Front.pulsewidth(0);
    RMotor_Back.pulsewidth(0.1);
    LMotor_Front.pulsewidth(0.1);
}
void Back()
{
    RMotor_EN = 1;
    LMotor_EN = 1;
    RMotor_Front.pulsewidth(0);
    LMotor_Front.pulsewidth(0);
    RMotor_Back.pulsewidth(0.05);
    LMotor_Back.pulsewidth(0.05);
}
void Break()
{
    RMotor_Front.pulsewidth(0);
    LMotor_Front.pulsewidth(0);
    RMotor_Back.pulsewidth(0.05);
    LMotor_Back.pulsewidth(0.05);
    wait(0.1);
    RMotor_EN = 0;
    LMotor_EN = 0;
}
void Stop()
{
    RMotor_EN = 0;
    LMotor_EN = 0;
    RMotor_Front.pulsewidth(0);
    LMotor_Front.pulsewidth(0);
    RMotor_Back.pulsewidth(0);
    LMotor_Back.pulsewidth(0);
}


int dis_L,dis_R,dis_F = 80;
int F_len;
int move,measure,flag=1;

void count()
{
    dis_F =100;
    dis_L =100;
    dis_R =100;
    move =0;
    measure =0;
    flag=1;
}

void backcount(){
    move = 1; measure =0;
    }
void Rightcount(){
    move = 2; measure =0;
    }
void Leftcount(){
    move = 3;   measure =0;
    }
int main()
{

    while(1) {

        //거리측정
        if(dis_F==F_sonic_F.read()/10) {
            dis_F = 80;}
        if(dis_R==F_sonic_R.read()/10) {
            dis_R = 90;}
        if(dis_L==F_sonic_L.read()/10) {
            dis_L = 90;}
                
        if(dis_F!=F_sonic_F.read()/10) {
            dis_F = F_sonic_F.read()/10;
        }
        if(dis_R!=F_sonic_R.read()/10) {
            dis_R = F_sonic_R.read()/10;
        }
        if(dis_L!=F_sonic_L.read()/10) {
            dis_L = F_sonic_L.read()/10;
        }
       
        printf("Front: %d\n",dis_F);
        printf("Right: %d\n",dis_L);
        printf("Left : %d\n",dis_R);
        printf("move : %d\n",move);

       /* if(dis_R <50) {
                if(dis_L <50){
                    if(dis_F >50){
                move =0;}}}
        else if(dis_L <50) {
                if(dis_R <50){
                    if(dt is_F >50){
                move =0;}}}*/
        
     
        switch(measure){
            case 0:
               if(dis_F >30) {
                if(dis_R <50) {
                if((dis_R + 5) < dis_L){
                move =2; measure =1;}
            } else if(dis_L <50) {
                if((dis_L + 5) < dis_R){
                move =3;measure =1;}
            } else {
                move =0; measure =1;
            }
            } else if(dis_F<30) {
            move =1; measure =1;
            }
            break;
            }
            
        
        if(dis_R <10){move=5;}
        else if(dis_L <10){ move =6;}
        else if((dis_R <25) &&(dis_L<25)){move =4;}
        else if(dis_R <35){move =2;}
        else if(dis_L <35){move =3;}
            
        switch(move) {
            case 0://Front
                Front();
                measure =0;
                break;

            case 1://Back
                Stop();
                measure =0;
                break;

            case 2://Right
                Turn_R();
                if (flag==1) {
                    flag=0;
                    motortime.attach(&count,1);
                }
                wait(1);
                break;

            case 3://Left
                Turn_L();
                if (flag==1) {
                    flag=0;
                    motortime.attach(&count,1);
                }
                wait(1);
                break;
           
           case 4://back
                Back();
                backmotortime.attach(&backcount,2);
                break;
           case 5://Right
                Back();
                Rightmotortime.attach(&Rightcount,2);
                break;
           case 6://back
                Back();
                Leftmotortime.attach(&Leftcount,2);
                break;
        }
    }

}