![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2014_Ensoul_Capstone
Dependencies: TextLCD Ultrasonic mbed BufferedSoftSerial
main.cpp
- Committer:
- leejong87
- Date:
- 2014-07-03
- Revision:
- 1:2859bfed20b4
- Parent:
- 0:d429f13fe4be
- Child:
- 2:c9740fccf3a7
File content as of revision 1:2859bfed20b4:
//Developer Kang, Lee #include "mbed.h" #include "Ultrasonic.h" 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 motortime; Timeout backmotortime; Timeout Rightmotortime; Timeout Leftmotortime; float a,b =0; float length; 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); } int dis_L,dis_R,dis_F = 80; int F_len; int move,measure,flag=1; void count() { dis_F =100; dis_L =100; dis_R =100; move =0; measure =0; flag=1; } void backcount(){ move = 1; measure =0; } void Rightcount(){ move = 2; measure =0; } void Leftcount(){ move = 3; measure =0; } int main() { while(1) { //거리측정 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; } printf("Front: %d\n",dis_F); printf("Right: %d\n",dis_L); printf("Left : %d\n",dis_R); printf("move : %d\n",move); /* 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; 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; } 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 Turn_R(); if (flag==1) { flag=0; motortime.attach(&count,1); } wait(1); break; case 3://Left Turn_L(); if (flag==1) { flag=0; motortime.attach(&count,1); } wait(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; } } }