2014_Ensoul_Capstone
Dependencies: TextLCD Ultrasonic mbed BufferedSoftSerial
main.cpp
- Committer:
- leejong87
- Date:
- 2014-07-08
- Revision:
- 5:58e37c4502ea
- Parent:
- 4:fd36ff807a91
File content as of revision 5:58e37c4502ea:
//Developer Kang, Lee #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); Ultrasonic F_sonic_L(p12,p12); Ultrasonic F_sonic_F(p8,p8); PwmOut RMotor_Front(p21); PwmOut LMotor_Front(p23); PwmOut RMotor_Back(p22); PwmOut LMotor_Back(p24); DigitalOut RMotor_EN(p5); DigitalOut LMotor_EN(p6); Timeout casemeasure; 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=1,measure=1,flag=1,clag =1; 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; 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; LMotor_EN = 1; RMotor_Back.pulsewidth(0); LMotor_Back.pulsewidth(0); RMotor_Front.pulsewidth(0.05); LMotor_Front.pulsewidth(0.05); } void Turn_R() { RMotor_EN = 1; LMotor_EN =1; LMotor_Front.pulsewidth(0); RMotor_Back.pulsewidth(0); LMotor_Back.pulsewidth(0.1); RMotor_Front.pulsewidth(0.1); } void Turn_L() { LMotor_EN = 1; RMotor_EN = 1; LMotor_Back.pulsewidth(0); RMotor_Front.pulsewidth(0); RMotor_Back.pulsewidth(0.1); LMotor_Front.pulsewidth(0.1); } void Back() { RMotor_EN = 1; LMotor_EN = 1; RMotor_Front.pulsewidth(0); LMotor_Front.pulsewidth(0); RMotor_Back.pulsewidth(0.05); LMotor_Back.pulsewidth(0.05); } void Break() { RMotor_Front.pulsewidth(0); LMotor_Front.pulsewidth(0); RMotor_Back.pulsewidth(0.05); LMotor_Back.pulsewidth(0.05); wait(0.1); RMotor_EN = 0; LMotor_EN = 0; } void Stop() { RMotor_EN = 0; LMotor_EN = 0; RMotor_Front.pulsewidth(0); LMotor_Front.pulsewidth(0); RMotor_Back.pulsewidth(0); LMotor_Back.pulsewidth(0); } void count() { dis_F =100; dis_L =100; dis_R =100; // measure =0; flag=1; } void backcount() { move = 1; // measure =0; } void Rightcount() { move = 2; // measure =0; } void Leftcount() { move = 3; // measure =0; } void measure_case1() { clag =1; move =1; //measure =1; } void measure_case0() { //measure =0; clag =1; } int main() { int Gc=0; 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) { xbeecount1=0; xbeecount2=0; xbeecount3=0; xbeeflag1 = 1; xbeeflag2 = 1; xbeeflag3 = 1; xbee1.printf("ATDB\r"); wait(0.05); xbee2.printf("ATDB\r"); wait(0.05); xbee3.printf("ATDB\r"); 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 ); rssi1_sum = 0; rssi2_sum = 0; rssi3_sum = 0; for (int i=0; i<10; i++) { rssi1_sum += rssi1[i]; rssi2_sum += rssi2[i]; rssi3_sum += rssi3[i]; } rssi1_avg = rssi1_sum / 10.0; rssi2_avg = rssi2_sum / 10.0; rssi3_avg = rssi3_sum / 10.0; d1 = rssi1_avg; d2 = rssi2_avg; d3 = rssi3_avg; /*if (d2 == d3) { //center if(d1 <= d3) { //front 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+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+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+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+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+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("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) { 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((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(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<9) { Gc++; } else { Gc=0; } } }