File content as of revision 3:59a400d203cc:

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

Ticker control;
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,measure=1,flag=1,clag =1;
int rssi1[5],rssi2[5],rssi3[5];

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;
}
void control_func()
{

    //거리측정
    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((distance >500.0) && ((angle*180/3.14) < 10.0) && ((angle*180/3.14) > -10.0)) {
                move = 1;// measure =0; flag =0;
            } else if((distance >500.0) && ((angle*180/3.14) > 10.0)) {
                move = 2;
            } else if((distance >500.0) && ((angle*180/3.14) < -10.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;
    }
}

int main()
{

    int Gc=0;

    control.attach(&control_func,0.1);
    xbee1.attach(&callback1);
    xbee2.attach(&callback2);
    xbee3.attach(&callback3);

    while(1) {
        xbee1.printf("+++");
        wait(0.05);
        xbee2.printf("+++");
        wait(0.05);
        xbee3.printf("+++");
        wait(1);
        xbeecount1=0;
        xbeeflag1 = 1;
        xbee1.printf("ATDB\r");
        wait(0.05);
        xbeecount2=0;
        xbeeflag2 = 1;
        xbee2.printf("ATDB\r");
        wait(0.05);
        xbeecount3=0;
        xbeeflag3 = 1;
        xbee3.printf("ATDB\r");
        wait(1);
        xbeeflag1 = 0;
        xbeeflag2 = 0;
        xbeeflag3 = 0;
        rssi1[Gc] = strtol( xbeebuf1, &stop , 16 );
        rssi2[Gc] = strtol( xbeebuf2, &stop , 16 );
        rssi3[Gc] = strtol( xbeebuf3, &stop , 16 );
        xbee1.printf("ATCN\r");
        wait(0.05);
        xbee2.printf("ATCN\r");
        wait(0.05);
        xbee3.printf("ATCN\r");
        wait(1);

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

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


        printf("RSSI:%ddBm,%ddBm,%ddBm\n",rssi1[Gc],rssi2[Gc],rssi3[Gc]);
        bt.printf("RSSI:%ddBm,%ddBm,%ddBm\n",rssi1[Gc],rssi2[Gc],rssi3[Gc]);
        printf("AVG :%fdBm,%fdBm,%fdBm\n",rssi1_avg,rssi2_avg,rssi3_avg);

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

        printf("DIST:%f, %f, %f\n\n",d1,d2,d3);

        if (d2 == d3) { //center
            if(d1 <= d3) { //front
                alpha = acos( (d3*d3+400.0*400.0-d2*d2)/(2*d3*400.0) );
                angle = 0;
                distance =  d3 * sin(alpha) / sin(1.57-angle);
            } else if (d1 > d3) { //rear
                alpha = acos( (d3*d3+400.0*400.0-d2*d2)/(2*d3*400.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+400.0*400.0-d2*d2)/(2*d3*400.0) );
                angle = 1.57 - atan( (d3 * sin(alpha))/(d3 * cos(alpha) - 200.0)); // +0 ~ +90
                distance = d3 * sin(alpha) / sin(1.57-angle);
            } else if (d1 > d3) { //rear
                alpha = acos( (d3*d3+400.0*400.0-d2*d2)/(2*d3*400.0) );
                angle = 1.57 + atan( (d3 * sin(alpha))/(d3 * cos(alpha) - 200.0)); // // +90 ~ +180
                distance = d3 * sin(alpha) / sin(1.57-angle);
            }

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

        printf("alpha : %f, angle : %f, distance : %f move = %d %d\n\n",alpha*180/3.14,angle*180/3.14,distance,move,measure);//++ left
bt.printf("alpha : %f, angle : %f, distance : %f move = %d %d\n\n",alpha*180/3.14,angle*180/3.14,distance,move,measure);//++ left
        if(Gc<2) {
            Gc++;
        } else {
            Gc=0;
        }
    }

}