![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2014_Ensoul_Capstone
Dependencies: TextLCD Ultrasonic mbed BufferedSoftSerial
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
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r fd36ff807a91 -r 58e37c4502ea main.cpp --- 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;