Revision 3:59a400d203cc, committed 2014-07-03
- Comitter:
- MR_Kang
- Date:
- Thu Jul 03 17:50:19 2014 +0000
- Parent:
- 2:c9740fccf3a7
- Child:
- 4:fd36ff807a91
- Commit message:
- ?
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BufferedSoftSerial.lib Thu Jul 03 17:50:19 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/BufferedSoftSerial/#a0fc30c0e231
--- a/main.cpp Thu Jul 03 07:34:06 2014 +0000
+++ b/main.cpp Thu Jul 03 17:50:19 2014 +0000
@@ -2,10 +2,12 @@
#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);
@@ -20,6 +22,7 @@
DigitalOut LMotor_EN(p6);
Ticker control;
+Timeout casemeasure;
Timeout motortime;
Timeout backmotortime;
Timeout Rightmotortime;
@@ -29,7 +32,7 @@
float length;
int dis_L,dis_R,dis_F = 80;
int F_len;
-int move,measure,flag=1;
+int move,measure=1,flag=1,clag =1;
int rssi1[5],rssi2[5],rssi3[5];
double d1, d2, d3, alpha, angle, distance; //정면, 좌측, 우측, 각도, 최종 각도, 최종 거리
@@ -137,35 +140,42 @@
LMotor_Back.pulsewidth(0);
}
-
-
-
void count()
{
dis_F =100;
dis_L =100;
dis_R =100;
- move =0;
- measure =0;
+ // measure =0;
flag=1;
}
void backcount()
{
move = 1;
- measure =0;
+ // measure =0;
}
void Rightcount()
{
move = 2;
- measure =0;
+ // measure =0;
}
void Leftcount()
{
move = 3;
- measure =0;
+ // measure =0;
}
+void measure_case1()
+{
+ clag =1;
+ move =1;
+ //measure =1;
+}
+void measure_case0()
+{
+ //measure =0;
+ clag =1;
+}
void control_func()
{
@@ -206,46 +216,57 @@
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;
+ }
+ 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;
}
-
-
- 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
@@ -254,7 +275,6 @@
flag=0;
motortime.attach(&count,1);
}
- wait(1);
break;
case 3://Left
@@ -263,7 +283,6 @@
flag=0;
motortime.attach(&count,1);
}
- wait(1);
break;
case 4://back
@@ -293,19 +312,19 @@
while(1) {
xbee1.printf("+++");
- wait(0.1);
+ wait(0.05);
xbee2.printf("+++");
- wait(0.1);
+ wait(0.05);
xbee3.printf("+++");
wait(1);
xbeecount1=0;
xbeeflag1 = 1;
xbee1.printf("ATDB\r");
- wait(0.1);
+ wait(0.05);
xbeecount2=0;
xbeeflag2 = 1;
xbee2.printf("ATDB\r");
- wait(0.1);
+ wait(0.05);
xbeecount3=0;
xbeeflag3 = 1;
xbee3.printf("ATDB\r");
@@ -317,9 +336,9 @@
rssi2[Gc] = strtol( xbeebuf2, &stop , 16 );
rssi3[Gc] = strtol( xbeebuf3, &stop , 16 );
xbee1.printf("ATCN\r");
- wait(0.1);
+ wait(0.05);
xbee2.printf("ATCN\r");
- wait(0.1);
+ wait(0.05);
xbee3.printf("ATCN\r");
wait(1);
@@ -327,17 +346,18 @@
rssi2_sum = 0;
rssi3_sum = 0;
- for (int i=0; i<5; i++) {
+ for (int i=0; i<3; 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 / 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;
@@ -379,12 +399,12 @@
}
}
- printf("alpha : %f, angle : %f, distance : %f\n\n",alpha*180/3.14,angle*180/3.14,distance);
-
- if(Gc>=4) {
+ 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;
- } else {
- Gc++;
}
}
Kang hokyoung
