舵機超聲波避障的單片機程序

ID:147905 · 發(fā)表于 2017-11-15 23:05

舵機超聲波避障和警燈閃爍
用別人的程序改的自己加了一些別的功能（我也是邊學邊做的這輛車可能有的地方不對大家多指點

）
有需要的可以拿去用，肯定成功
單片機用的是 89c52
超聲波是rs04
電機驅動L298N
程序注釋的已經很詳細了！

/******************************************************************************
2017.11.14修改
刪除額外數碼管功能
加入閃爍警燈程序
******************************************************************************/
#include <AT89x52.H>
#include <intrins.h>
#define led1 {P2_0=1,P2_1=0;}
#define led2 {P2_0=0,P2_1=1;}
/**********如果io口不夠用可以去掉2^4~2^7口將298使能接口接高電平即可***********/
#define Right_moto_go {P0_0=1,P0_1=0,P0_2=1,P0_3=0,P2_7=1,P2_6=1,P2_5=1,P2_4=1;} //右邊電機向前走
#define Right_moto_back {P0_0=0,P0_1=1,P0_2=0,P0_3=1,P2_7=1,P2_6=1,P2_5=1,P2_4=1;} //右邊電機向后走
#define Right_moto_Stop {P0_0=1,P0_1=1,P0_2=1,P0_3=1,P2_7=1,P2_6=1,P2_5=1,P2_4=1;} //右邊電機停轉
#define Left_moto_go {P0_4=0,P0_5=1,P0_6=0,P0_7=1,P2_7=1,P2_6=1,P2_5=1,P2_4=1;} //左邊電機向前走
#define Left_moto_back {P0_4=1,P0_5=0,P0_6=1,P0_7=0,P2_7=1,P2_6=1,P2_5=1,P2_4=1;} //左邊電機向后轉
#define Left_moto_Stop {P0_4=1,P0_5=1,P0_6=1,P0_7=1,P2_7=1,P2_6=1,P2_5=1,P2_4=1;} //左邊電機停轉
/*****************************************************************************/
#define Sevro_moto_pwm P1_2 //接舵機信號端輸入PWM信號調節(jié)速度
#define ECHO P1_1 //超聲波接口定義
#define TRIG P1_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 run(void)
{
Left_moto_go; //左電機往前走
Right_moto_go ; //右電機往前走
}
//前速后退
void backrun(void)
{
Left_moto_back ; //左電機往前走
Right_moto_back ; //右電機往前走
}
//左轉
void leftrun(void)
{
Right_moto_go ; //右電機往前走 ;
Left_moto_Stop ; //左電機停止
}
//右轉
void rightrun(void)
{
Left_moto_go ; //左電機往前走
Right_moto_Stop ; //右電機停止
}
//停
void stoprun(void)
{
Left_moto_Stop ; //左電機往前走
Right_moto_Stop ; //右電機往前走
}
//****************************去除額外功能********************************/
// void Display(void) //掃描數碼管
// {
// if(posit==0)
// {P3=(discode[disbuff[posit]])&0x7f;}//產生點
// else
// {P3=discode[disbuff[posit]];}
// if(posit==0)
// { P1_3=0;P1_4=1;P1_5=1;}
// if(posit==1)
// {P1_3=1;P1_4=0;P1_5=1;}
// if(posit==2)
// {P1_3=1;P1_4=1;P1_5=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 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<=3500);//原4000
}
else
{
leftrun(); //車的左邊比車的右邊距離大左轉
timer=0;
while(timer<=3500);//原4000
}
}
/*調節(jié)push_val_left的值改變電機轉速,占空比 */
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();
// }
}
/************************************************************************/
/*****************************閃爍燈中斷延時函數****************************/
void delay1()
{
unsigned char i,j,k;
for(i=1;i>0;i--)
for(j=100;j>0;j--)
for(k=250;k>0;k--);
}
void shansuo()
{
led1;
delay1();
led2;
delay1();
}
/***********************************************************************/
/*********************************************************************/
/*--主函數--*/
void main(void)
{
stoprun();
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) /*無限循環(huán)*/
{
shansuo(); //閃爍燈函數
if(timer>=200) //100MS檢測啟動檢測一次原來500
{
timer=0;
StartModule(); //啟動檢測
Conut(); //計算距離
if(S<35) //距離小于20CM
{
stoprun(); //小車停止
COMM(); //方向函數
}
else
if(S>40) //距離大于，35CM往前走
run();
}
}
}