File content as of revision 5:58e37c4502ea:

//Developer Kang, Lee

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

Serial xbee1(p9, p10); // tx, rx
Serial xbee2(p13, p14);
Serial xbee3(p28, p27);
SoftSerial bt(p29,p30);

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 casemeasure;
Timeout motortime;
Timeout backmotortime;
Timeout Rightmotortime;
Timeout Leftmotortime;

float a,b =0;
float length;
int dis_L,dis_R,dis_F = 80;
int F_len;
int move=1,measure=1,flag=1,clag =1;
int rssi1[10],rssi2[10],rssi3[10];

double d1, d2, d3, alpha, angle, distance; //정면, 좌측, 우측, 각도, 최종 각도, 최종 거리
double rssi1_sum,rssi2_sum,rssi3_sum,rssi1_avg,rssi2_avg,rssi3_avg;

char xbeechar1, xbeebuf1[3], xbeechar2, xbeebuf2[3] ,xbeechar3, xbeebuf3[3];
int xbeeflag1=0, xbeecount1=0, xbeeflag2=0, xbeecount2=0, xbeeflag3=0, xbeecount3=0;

char *stop;

void callback1()
{
    xbeechar1 = xbee1.getc();
    if(xbeechar1 == '\r') {
        //pc.printf("\n");
    } else if ((xbeechar1 != 'T')&&(xbeechar1 != 'O')&&(xbeechar1 != 'K')) {
        //pc.printf("%c", xbeechar1);
    }
    if(xbeeflag1 == 1) {
        xbeebuf1[xbeecount1++]=xbeechar1;
    }
}
void callback2()
{
    xbeechar2 = xbee2.getc();
    if(xbeechar2 == '\r') {
        //pc.printf("\n");

    } else if ((xbeechar2 != 'T')&&(xbeechar2 != 'O')&&(xbeechar2 != 'K')) {
        //pc.printf("%c", xbeechar2);
    }
    if(xbeeflag2 == 1) {
        xbeebuf2[xbeecount2++]=xbeechar2;
    }
}
void callback3()
{
    xbeechar3 = xbee3.getc();
    if(xbeechar3 == '\r') {
        //pc.printf("\n");

    } else if ((xbeechar3 != 'T')&&(xbeechar3 != 'O')&&(xbeechar3 != 'K')) {
        //pc.printf("%c", xbeechar3);
    }
    if(xbeeflag3 == 1) {
        xbeebuf3[xbeecount3++]=xbeechar3;
    }
}

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

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

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

void measure_case1()
{
    clag =1;
    move =1;
    //measure =1;
}
void measure_case0()
{
    //measure =0;
    clag =1;
}

int main()
{

    int Gc=0;

    xbee1.attach(&callback1);
    xbee2.attach(&callback2);
    xbee3.attach(&callback3);

    xbee1.printf("+++");
    wait(0.05);
    xbee2.printf("+++");
    wait(0.05);
    xbee3.printf("+++");
    wait(1);

    while(1) {

        xbeecount1=0;
        xbeecount2=0;
        xbeecount3=0;
        xbeeflag1 = 1;
        xbeeflag2 = 1;
        xbeeflag3 = 1;
        xbee1.printf("ATDB\r");
        wait(0.05);
        xbee2.printf("ATDB\r");
        wait(0.05);
        xbee3.printf("ATDB\r");
        wait(0.05);
        xbeeflag1 = 0;
        xbeeflag2 = 0;
        xbeeflag3 = 0;
        //xbee1.printf("ATCN\r");
        //wait(0.05);
        //xbee2.printf("ATCN\r");
        //wait(0.05);
        //xbee3.printf("ATCN\r");

        rssi1[Gc] = strtol( xbeebuf1, &stop , 16 );
        rssi2[Gc] = strtol( xbeebuf2, &stop , 16 );
        rssi3[Gc] = strtol( xbeebuf3, &stop , 16 );

        rssi1_sum = 0;
        rssi2_sum = 0;
        rssi3_sum = 0;

        for (int i=0; i<10; i++) {
            rssi1_sum += rssi1[i];
            rssi2_sum += rssi2[i];
            rssi3_sum += rssi3[i];
        }
        rssi1_avg = rssi1_sum / 10.0;
        rssi2_avg = rssi2_sum / 10.0;
        rssi3_avg = rssi3_sum / 10.0;

        d1 = rssi1_avg;
        d2 = rssi2_avg;
        d3 = rssi3_avg;

        /*if (d2 == d3) { //center
            if(d1 <= d3) { //front
                alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.0) );
                angle = 0;
                distance =  d3 * sin(alpha) / sin(1.57-angle);
            } else if (d1 > d3) { //rear
                alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.0) );
                angle = 3.14;
                distance =  d3 * sin(alpha) / sin(1.57-angle);
            }
        } else if(d2 < d3) { //left
            if(d1 <= d3) { //front
                alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.0) );
                angle = 1.57 - atan( (d3 * sin(alpha))/(d3 * cos(alpha) - 100.0)); // +0 ~ +90
                distance = d3 * sin(alpha) / sin(1.57-angle);
            } else if (d1 > d3) { //rear
                alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.0) );
                angle = 1.57 + atan( (d3 * sin(alpha))/(d3 * cos(alpha) - 100.0)); // // +90 ~ +180
                distance = d3 * sin(alpha) / sin(1.57-angle);
            }

        } else if(d2 > d3) { //right
            if(d1 <= d2) { //front
                alpha = acos( (d2*d2+200.0*200.0-d3*d3)/(2*d2*200.0) );
                angle = -1.57 + atan( (d2 * sin(alpha))/(d2 * cos(alpha) - 100.0)) ; // -0 ~ -90
                distance = d2 * sin(alpha) / sin(1.57+angle);
            } else if (d1 > d2) { //rear
                alpha = acos( (d2*d2+200.0*200.0-d3*d3)/(2*d2*200.0) );
                angle = -1.57 - atan( (d2 * sin(alpha))/(d2 * cos(alpha) - 100.0)); // -90 ~ -180
                distance = d2 * sin(alpha) / sin(1.57+angle);
            }
        }
        */
        //printf("RSSI:%ddBm,%ddBm,%ddBm   AVG :%fdBm,%fdBm,%fdBm    alpha : %f, angle : %f, distance : %f move = %d   \r",rssi1[Gc],rssi2[Gc],rssi3[Gc],rssi1_avg,rssi2_avg,rssi3_avg,alpha*180/3.14,angle*180/3.14,distance,move);
        //bt.printf("RSSI:%ddBm,%ddBm,%ddBm   AVG :%fdBm,%fdBm,%fdBm    alpha : %f, angle : %f, distance : %f move = %d   \r",rssi1[Gc],rssi2[Gc],rssi3[Gc],rssi1_avg,rssi2_avg,rssi3_avg,alpha*180/3.14,angle*180/3.14,distance,move);
        printf("RSSI:%ddBm,%ddBm,%ddBm   AVG :%fdBm,%fdBm,%fdBm    move = %d   \r",rssi1[Gc],rssi2[Gc],rssi3[Gc],rssi1_avg,rssi2_avg,rssi3_avg,move);
        bt.printf("RSSI:%ddBm,%ddBm,%ddBm   AVG :%fdBm,%fdBm,%fdBm    move = %d   \r",rssi1[Gc],rssi2[Gc],rssi3[Gc],rssi1_avg,rssi2_avg,rssi3_avg,move);

        //거리측정
        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;
        }

        /* 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;
                        }
                    } else if(dis_L <50) {
                        if((dis_L + 5) < dis_R) {
                            move =3;
                        }
                    } else {
                        move =0;
                    }
                } else if(dis_F<30) {
                    move =1;
                }
                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;
                }
                if(clag ==1) {
                    clag =0;
                    casemeasure.attach(&measure_case1,3.0);
                }
                break;

            case 1:
                if((d1 >50.0) && ((d3 - d2) < 3.0) && ((d3 - d2) > -3.0)) {
                    move = 1;// measure =0; flag =0;

                } else if((d1 >50.0) && ((d3 - d2) > 3.0)) {
                    move = 2;
                } else if((d1 >50.0) && ((d3 - d2) < -3.0)) {
                    move = 3;
                }

                /* if((distance >500.0) && ((angle*180/3.14) < 20.0) && ((angle*180/3.14) > -20.0)) {
                    move = 1;// measure =0; flag =0;
                } else if((distance >500.0) && ((angle*180/3.14) > 20.0)) {
                    move = 2;
                } else if((distance >500.0) && ((angle*180/3.14) < -20.0)) {
                    move = 3;
                }
                if(clag ==1){
                     if(distance >500.0) {
                         clag =0;
                         casemeasure.attach(&measure_case0,1);
                     }
                 }*/
                break;
        }
        switch(move) {
            case 0://Front
                Front();
                break;

            case 1://Back
                Stop();
                break;

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

            case 3://Left
                Turn_L();
                if (flag==1) {
                    flag=0;
                    motortime.attach(&count,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;
        }

        if(Gc<9) {
            Gc++;
        } else {
            Gc=0;
        }
    }

}