Kang hokyoung
2014_Ensoul_Capstone
Dependencies: TextLCD Ultrasonic mbed BufferedSoftSerial
Diff: main.cpp
- Revision:
- 4:fd36ff807a91
- Parent:
- 3:59a400d203cc
- Child:
- 5:58e37c4502ea
--- a/main.cpp Thu Jul 03 17:50:19 2014 +0000
+++ b/main.cpp Fri Jul 04 06:34:04 2014 +0000
@@ -21,7 +21,6 @@
DigitalOut RMotor_EN(p5);
DigitalOut LMotor_EN(p6);
-Ticker control;
Timeout casemeasure;
Timeout motortime;
Timeout backmotortime;
@@ -32,7 +31,7 @@
float length;
int dis_L,dis_R,dis_F = 80;
int F_len;
-int move,measure=1,flag=1,clag =1;
+int move=1,measure=1,flag=1,clag =1;
int rssi1[5],rssi2[5],rssi3[5];
double d1, d2, d3, alpha, angle, distance; //정면, 좌측, 우측, 각도, 최종 각도, 최종 거리
@@ -145,24 +144,24 @@
dis_F =100;
dis_L =100;
dis_R =100;
- // 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()
@@ -176,232 +175,224 @@
//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);
+ xbee1.printf("+++");
+ wait(0.05);
+ xbee2.printf("+++");
+ wait(0.05);
+ xbee3.printf("+++");
+ wait(1);
+
while(1) {
- xbee1.printf("+++");
- wait(0.05);
- xbee2.printf("+++");
- wait(0.05);
- xbee3.printf("+++");
- wait(1);
+
xbeecount1=0;
+ xbeecount2=0;
+ xbeecount3=0;
xbeeflag1 = 1;
+ xbeeflag2 = 1;
+ xbeeflag3 = 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);
+ wait(0.05);
xbeeflag1 = 0;
xbeeflag2 = 0;
xbeeflag3 = 0;
+ //xbee1.printf("ATCN\r");
+ //wait(0.05);
+ //xbee2.printf("ATCN\r");
+ //wait(0.05);
+ //xbee3.printf("ATCN\r");
+
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++) {
+ for (int i=0; i<5; 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;
-
+ 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]);
- 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);
+ d1 = (rssi1_avg-20)*10;
+ d2 = (rssi2_avg-20)*10;
+ d3 = (rssi3_avg-20)*10;
if (d2 == d3) { //center
if(d1 <= d3) { //front
- alpha = acos( (d3*d3+400.0*400.0-d2*d2)/(2*d3*400.0) );
+ alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.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) );
+ alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.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
+ alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.0) );
+ angle = 1.57 - atan( (d3 * sin(alpha))/(d3 * cos(alpha) - 100.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
+ alpha = acos( (d3*d3+200.0*200.0-d2*d2)/(2*d3*200.0) );
+ angle = 1.57 + atan( (d3 * sin(alpha))/(d3 * cos(alpha) - 100.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
+ alpha = acos( (d2*d2+200.0*200.0-d3*d3)/(2*d2*200.0) );
+ angle = -1.57 + atan( (d2 * sin(alpha))/(d2 * cos(alpha) - 100.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
+ alpha = acos( (d2*d2+200.0*200.0-d3*d3)/(2*d2*200.0) );
+ angle = -1.57 - atan( (d2 * sin(alpha))/(d2 * cos(alpha) - 100.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) {
+ 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);
+
+ //거리측정
+ 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) < 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(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;
+ }
+
+ if(Gc<4) {
Gc++;
} else {
Gc=0;
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 03 Jul 2014 |
| Imports: | 4 |
| Forks: | 0 |
| Commits: | 6 |
| Dependents: | 0 |
| Dependencies: | 4 |
| Followers: | 2 |
