Revision 5:58e37c4502ea, committed 2014-07-08

Comitter:

leejong87

Date:

Tue Jul 08 09:40:12 2014 +0000

Parent:

4:fd36ff807a91

Commit message:

2014/07/08

Changed in this revision

--- a/main.cpp	Fri Jul 04 06:34:04 2014 +0000
+++ b/main.cpp	Tue Jul 08 09:40:12 2014 +0000
@@ -32,7 +32,7 @@
 int dis_L,dis_R,dis_F = 80;
 int F_len;
 int move=1,measure=1,flag=1,clag =1;
-int rssi1[5],rssi2[5],rssi3[5];
+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;
@@ -223,20 +223,20 @@
         rssi2_sum = 0;
         rssi3_sum = 0;
 
-        for (int i=0; i<5; i++) {
+        for (int i=0; i<10; 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;
+        rssi1_avg = rssi1_sum / 10.0;
+        rssi2_avg = rssi2_sum / 10.0;
+        rssi3_avg = rssi3_sum / 10.0;
 
-        d1 = (rssi1_avg-20)*10;
-        d2 = (rssi2_avg-20)*10;
-        d3 = (rssi3_avg-20)*10;
+        d1 = rssi1_avg;
+        d2 = rssi2_avg;
+        d3 = rssi3_avg;
 
-        if (d2 == d3) { //center
+        /*if (d2 == d3) { //center
             if(d1 <= d3) { //front
                 alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.0) );
                 angle = 0;
@@ -268,9 +268,11 @@
                 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    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) {
@@ -338,14 +340,23 @@
                 break;
 
             case 1:
-                if((distance >500.0) && ((angle*180/3.14) < 20.0) && ((angle*180/3.14) > -20.0)) {
+                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(clag ==1){
                      if(distance >500.0) {
                          clag =0;
                          casemeasure.attach(&measure_case0,1);
@@ -392,7 +403,7 @@
                 break;
         }
 
-        if(Gc<4) {
+        if(Gc<9) {
             Gc++;
         } else {
             Gc=0;