我給你來個 超聲波避障 程序試試
- #include <AT89x51.H>
- #include <intrins.h>
- #define Sevro_moto_pwm P2_7 //接舵機信號端輸入PWM信號調節速度
- #define ECHO P2_4 //超聲波接口定義
- #define TRIG P2_5 //超聲波接口定義
- #define Left_moto_go {P1_0=1,P1_1=0,P1_2=1,P1_3=0;} //左邊兩個電機向前走
- #define Left_moto_back {P1_0=0,P1_1=1,P1_2=0,P1_3=1;} //左邊兩個電機向后轉
- #define Left_moto_Stop {P1_0=0,P1_1=0,P1_2=0,P1_3=0;} //左邊兩個電機停轉
- #define Right_moto_go {P1_4=1,P1_5=0,P1_6=1,P1_7=0;} //右邊兩個電機向前走
- #define Right_moto_back {P1_4=0,P1_5=1,P1_6=0,P1_7=1;} //右邊兩個電機向前走
- #define Right_moto_Stop {P1_4=0,P1_5=0,P1_6=0,P1_7=0;} //右邊兩個電機停轉
- unsigned char const discode[] ={ 0xC0,0xF9,0xA4,0xB0,0x99,0x92,0x82,0xF8,0x80,0x90,0xBF,0xff/*-*/};
- unsigned char const positon[3]={ 0xfe,0xfd,0xfb};
- unsigned char disbuff[4] ={ 0,0,0,0,};
- unsigned char posit=0;
- unsigned char pwm_val_left = 0;//變量定義
- unsigned char push_val_left =14;//舵機歸中,產生約,1.5MS 信號
- unsigned long S=0;
- unsigned long S1=0;
- unsigned long S2=0;
- unsigned long S3=0;
- unsigned long S4=0;
- unsigned int time=0; //時間變量
- unsigned int timer=0; //延時基準變量
- unsigned char timer1=0; //掃描時間變量
- void delay(unsigned int k) //延時函數
- {
- unsigned int x,y;
- for(x=0;x<k;x++)
- for(y=0;y<2000;y++);
- }
- void Display(void) //掃描數碼管
- {
- if(posit==0)
- {P0=(discode[disbuff[posit]])&0x7f;}//產生點
- else
- {P0=discode[disbuff[posit]];}
- if(posit==0)
- { P2_1=0;P2_2=1;P2_3=1;}
- if(posit==1)
- {P2_1=1;P2_2=0;P2_3=1;}
- if(posit==2)
- {P2_1=1;P2_2=1;P2_3=0;}
- if(++posit>=3)
- posit=0;
- }
- void StartModule() //啟動測距信號
- {
- TRIG=1;
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- _nop_();
- TRIG=0;
- }
- void Conut(void) //計算距離
- {
- while(!ECHO); //當RX為零時等待
- TR0=1; //開啟計數
- while(ECHO); //當RX為1計數并等待
- TR0=0; //關閉計數
- time=TH0*256+TL0; //讀取脈寬長度
- TH0=0;
- TL0=0;
- S=(time*1.7)/100; //算出來是CM
- disbuff[0]=S%1000/100; //更新顯示
- disbuff[1]=S%1000%100/10;
- disbuff[2]=S%1000%10 %10;
- }
- //前速前進
- void run(void)
- { Left_moto_back ;
- //Left_moto_go ; //左電機往前走
- Right_moto_go ; //右電機往前走
- }
- //前速后退
- void backrun(void)
- { Left_moto_go ;
- //Left_moto_back ; //左電機往前走
- Right_moto_back ; //右電機往前走
- }
- //左轉
- void leftrun(void)
- { Left_moto_go ;
- //Left_moto_back ; //左電機往前走
- Right_moto_go ; //右電機往前走
- }
- //右轉
- void rightrun(void)
- { Left_moto_back ;
- //Left_moto_go ; //左電機往前走
- Right_moto_back ; //右電機往前走
- }
- /************************************************************************/
- //STOP
- void stoprun(void)
- {
- Left_moto_Stop ; //左電機停走
- Right_moto_Stop ; //右電機停走
- }
- /************************************************************************/
- void COMM( void )
- {
-
-
- push_val_left=5; //舵機向左轉90度
- timer=0;
- while(timer<=4000); //延時400MS讓舵機轉到其位置
- StartModule(); //啟動超聲波測距
- Conut(); //計算距離
- S2=S;
-
- push_val_left=23; //舵機向右轉90度
- timer=0;
- while(timer<=4000); //延時400MS讓舵機轉到其位置
- StartModule(); //啟動超聲波測距
- Conut(); //計算距離
- S4=S;
-
- push_val_left=14; //舵機歸中
- timer=0;
- while(timer<=4000); //延時400MS讓舵機轉到其位置
- StartModule(); //啟動超聲波測距
- Conut(); //計算距離
- S1=S;
- if((S2<20)||(S4<20)) //只要左右各有距離小于,20CM小車后退
- {
- backrun(); //后退
- timer=0;
- while(timer<=4000);
- }
-
- if(S2>S4)
- {
- rightrun(); //車的左邊比車的右邊距離小 右轉
- timer=0;
- while(timer<=4000);
- }
- else
- {
- leftrun(); //車的左邊比車的右邊距離大 左轉
- timer=0;
- while(timer<=4000);
- }
-
- }
- void pwm_Servomoto(void)
- {
-
- if(pwm_val_left<=push_val_left)
- Sevro_moto_pwm=1;
- else
- Sevro_moto_pwm=0;
- if(pwm_val_left>=200)
- pwm_val_left=0;
-
- }
- ///*TIMER1中斷服務子函數產生PWM信號*/
- void time1()interrupt 3 using 2
- {
- TH1=(65536-100)/256; //100US定時
- TL1=(65536-100)%256;
- timer++; //定時器100US為準。在這個基礎上延時
- pwm_val_left++;
- pwm_Servomoto();
- timer1++; //2MS掃一次數碼管
- if(timer1>=20)
- {
- timer1=0;
- Display();
- }
- }
- ///*TIMER0中斷服務子函數產生PWM信號*/
- void timer0()interrupt 1 using 0
- {
-
- }
- void main(void)
- {
- TMOD=0X11;
- TH1=(65536-100)/256; //100US定時
- TL1=(65536-100)%256;
- TH0=0;
- TL0=0;
- TR1= 1;
- ET1= 1;
- ET0= 1;
- EA = 1;
- delay(100);
- push_val_left=14; //舵機歸中
- while(1) /*無限循環*/
- {
- if(timer>=1000) //100MS檢測啟動檢測一次
- {
- timer=0;
- StartModule(); //啟動檢測
- Conut(); //計算距離
- if(S<30) //距離小于20CM
- {
- stoprun(); //小車停止
- COMM(); //方向函數
- }
- else
- if(S>35) //距離大于,30CM往前走
- run();
- }
- }
- }
-
復制代碼 |