Diff: main.cpp

Revision:

2:c9740fccf3a7

Parent:

1:2859bfed20b4

Child:

3:59a400d203cc

--- a/main.cpp	Thu Jul 03 05:24:13 2014 +0000
+++ b/main.cpp	Thu Jul 03 07:34:06 2014 +0000
@@ -3,6 +3,10 @@
 #include "mbed.h"
 #include "Ultrasonic.h"
 
+Serial xbee1(p9, p10); // tx, rx
+Serial xbee2(p13, p14);
+Serial xbee3(p28, p27);
+
 AnalogIn Sharp(p20);
 Ultrasonic F_sonic_R(p11,p11);
 Ultrasonic F_sonic_L(p12,p12);
@@ -14,12 +18,67 @@
 PwmOut LMotor_Back(p24);
 DigitalOut RMotor_EN(p5);
 DigitalOut LMotor_EN(p6);
+
+Ticker control;
 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,flag=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;
@@ -79,9 +138,7 @@
 }
 
 
-int dis_L,dis_R,dis_F = 80;
-int F_len;
-int move,measure,flag=1;
+
 
 void count()
 {
@@ -93,119 +150,241 @@
     flag=1;
 }
 
-void backcount(){
-    move = 1; measure =0;
+void backcount()
+{
+    move = 1;
+    measure =0;
+}
+void Rightcount()
+{
+    move = 2;
+    measure =0;
+}
+void Leftcount()
+{
+    move = 3;
+    measure =0;
+}
+
+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;
     }
-void Rightcount(){
-    move = 2; measure =0;
+
+    /* 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;
     }
-void Leftcount(){
-    move = 3;   measure =0;
+
+
+    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;
+    }
+}
+
 int main()
 {
 
-    while(1) {
+    int Gc=0;
 
-        //거리측정
-        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);
+    control.attach(&control_func,0.1);
+    xbee1.attach(&callback1);
+    xbee2.attach(&callback2);
+    xbee3.attach(&callback3);
 
-       /* 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;
+    while(1) {
+        xbee1.printf("+++");
+        wait(0.1);
+        xbee2.printf("+++");
+        wait(0.1);
+        xbee3.printf("+++");
+        wait(1);
+        xbeecount1=0;
+        xbeeflag1 = 1;
+        xbee1.printf("ATDB\r");
+        wait(0.1);
+        xbeecount2=0;
+        xbeeflag2 = 1;
+        xbee2.printf("ATDB\r");
+        wait(0.1);
+        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.1);
+        xbee2.printf("ATCN\r");
+        wait(0.1);
+        xbee3.printf("ATCN\r");
+        wait(1);
+
+        rssi1_sum = 0;
+        rssi2_sum = 0;
+        rssi3_sum = 0;
+
+        for (int i=0; i<5; i++) {
+            rssi1_sum += rssi1[i];
+            rssi2_sum += rssi2[i];
+            rssi3_sum += rssi3[i];
+        }
+        rssi1_avg = rssi1_sum / 5.0;
+        rssi2_avg = rssi2_sum / 5.0;
+        rssi3_avg = rssi3_sum / 5.0;
+
+
+        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);
             }
-            break;
+        } 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);
             }
-            
-        
-        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;
+        } 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);
+            }
+        }
 
-            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;
+        printf("alpha : %f, angle : %f, distance : %f\n\n",alpha*180/3.14,angle*180/3.14,distance);
+
+        if(Gc>=4) {
+            Gc=0;
+        } else {
+            Gc++;
         }
     }