兩路4輪舵機避障小車的單片機程序,已優(yōu)化,自行改造

ID:1091382 · 發(fā)表于 2023-8-28 16:21

兩路小車改的四輪小車，原先的兩輪轉(zhuǎn)向極不穩(wěn)定，行走也跑偏，不好玩，后來改的四輪，升級7.4v鋰電動力，程序里面調(diào)速前進（20,20），行走、轉(zhuǎn)向很穩(wěn)定。原理都一樣，無非多兩個tt電機，左右輪電機線分別接一起。程序優(yōu)化了一下能完美避障，距離太近還能自動后退。
程序需要完整下載，里面有個H文件一起加c語言才能用。單獨復制沒有定義引腳燒錄不了的。
制作出來的實物圖如下：

單片機源程序如下:

/*********************************************************************************
* 【外部晶振】： 11.0592mhz
* 【主控芯片】： STC89C52
* 【編譯環(huán)境】： Keil μVisio4
* Copyright(C) QXMBOT
* All rights reserved
***********************************************************************************
* 【接線說明】：請參考資料內(nèi)圖文說明書
* 【注意事項】：避免小車撞向障礙物或小車輪子堵轉(zhuǎn)
**********************************************************************************/
#include <reg52.h>//51頭文件
#include <QX_A11.h>//QX_A11智能小車配置文件
#include <intrins.h>
bit Timer1Overflow; //計數(shù)器1溢出標志位
unsigned char disbuff[4]={0,0,0,0};//用于分別存放距離的值米，厘米，毫米
unsigned int LeftDistance = 0, RightDistance = 0, FrontDistance = 0; //云臺測距距離緩存
unsigned int DistBuf[5] = {0};//distance data buffer
unsigned char pwm_val_left,pwm_val_right; //中間變量，只是此處用戶請勿修改。
unsigned char pwm_left,pwm_right; //定義PWM輸出高電平的時間的變量。用戶操作PWM的變量。左右（）里面的值
#define PWM_DUTY 200 //（用于舵機時不可修改）定義PWM的周期，數(shù)值為定時器0溢出周期，假如定時器溢出時間為100us，則PWM周期為20ms。
#define PWM_HIGH_MIN 70 //限制PWM輸出的最小占空比。用戶請勿修改。
#define PWM_HIGH_MAX PWM_DUTY //限制PWM輸出的最大占空比。用戶請勿修改。
void Timer0_Init(void); //定時器0初始化
void Timer1_Init(void);//定時器1初始化
void LoadPWM(void);//裝入PWM輸出值
void Delay_Ms(unsigned int ms);//毫秒級延時函數(shù)
void forward(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小車前進
void left_run(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小車左轉(zhuǎn)
void right_run(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小車右轉(zhuǎn)
void back_run(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小車后退
void stop(void);//QX_A11智能小車停車
void Delay18450us(void); //@11.0592MHz
void Delay1550us(void); //@11.0592MHz
void Delay19400us(void); //@11.0592MHz
void Delay600us(void); //@11.0592MHz
void Delay17500us(void); //@11.0592MHz
void Delay2500us(void); //@11.0592MHz
void QXMBOT_bubble(unsigned int *a,unsigned char n);//冒泡排序
void QXMBOT_ServoFront(void);//舵機向前
void QXMBOT_ServoLeft(void);//舵機左轉(zhuǎn)
void QXMBOT_ServoRight(void);//舵機右轉(zhuǎn)
void QXMBOT_PTZ_Avoid(unsigned int val);//舵機云臺避障
unsigned int QXMBOT_GetDistance(void);//獲取超聲波距離
void QXMBOT_TrigUltrasonic(void);// 觸發(fā)超聲波
unsigned int QXMBOT_RefreshDistance(void);//超聲波測距
/*主函數(shù)*/
void main(void)
{
Timer0_Init();//定時0初始化
Timer1_Init();//定時0初始化
EA_on; //開中斷
QXMBOT_ServoFront();//舵機向前
Delay_Ms(1000);
forward(20,50);//前進，forward全部調(diào)成（20,20）可調(diào)速。7.4v情況下
while(1)
{
QXMBOT_PTZ_Avoid(300);//舵機云臺避障，LCD1602顯示距離,設置觸發(fā)距離300毫米
}
}
/*********************************************
QX_A11智能小車前進
入口參數(shù)：LeftSpeed，RightSpeed
出口參數(shù): 無
說明：LeftSpeed，RightSpeed分別設置左右車輪轉(zhuǎn)速
**********************************************/
void forward(unsigned char LeftSpeed,unsigned char RightSpeed)
{
pwm_left = LeftSpeed,pwm_right = RightSpeed;//設置速度
left_motor_go; //左電機前進
right_motor_go; //右電機前進
}
/*小車左轉(zhuǎn)*/
/*********************************************
QX_A11智能小車左轉(zhuǎn)
入口參數(shù)：LeftSpeed，RightSpeed
出口參數(shù): 無
說明：LeftSpeed，RightSpeed分別設置左右車輪轉(zhuǎn)速
**********************************************/
void left_run(unsigned char LeftSpeed,unsigned char RightSpeed)
{
pwm_left = LeftSpeed,pwm_right = RightSpeed;//設置速度
left_motor_back; //左電機后退
right_motor_go; //右電機前進
}
/*********************************************
QX_A11智能小車右轉(zhuǎn)
入口參數(shù)：LeftSpeed，RightSpeed
出口參數(shù): 無
說明：LeftSpeed，RightSpeed分別設置左右車輪轉(zhuǎn)速
**********************************************/
void right_run(unsigned char LeftSpeed,unsigned char RightSpeed)
{
pwm_left = LeftSpeed,pwm_right = RightSpeed;//設置速度
right_motor_back;//右電機后退
left_motor_go; //左電機前進
}
/*********************************************
QX_A11智能小車后退
入口參數(shù)：LeftSpeed，RightSpeed
出口參數(shù): 無
說明：LeftSpeed，RightSpeed分別設置左右車輪轉(zhuǎn)速
**********************************************/
void back_run(unsigned char LeftSpeed,unsigned char RightSpeed)
{
pwm_left = LeftSpeed,pwm_right = RightSpeed;//設置速度
right_motor_back;//右電機后退
left_motor_back; //左電機后退
}
/*********************************************
QX_A11智能小車停車
入口參數(shù)：無
出口參數(shù): 無
**********************************************/
void stop(void)
{
left_motor_stops;
right_motor_stops;
}
/*====================================
函數(shù)：void Delay_Ms(INT16U ms)
參數(shù)：ms，毫秒延時形參
描述：12T 51單片機自適應主時鐘毫秒級延時函數(shù)
====================================*/
void Delay_Ms(unsigned int ms)
{
unsigned int i;
do{
i = MAIN_Fosc / 96000;
while(--i); //96T per loop
}while(--ms);
}
/*舵機方波延時朝向小車正前方*/
void Delay1550us() //@11.0592MHz
{
unsigned char i, j;
i = 3;
j = 196;
do
{
while (--j);
} while (--i);
}
void Delay18450us() //@11.0592MHz
{
unsigned char i, j;
_nop_();
i = 34;
j = 16;
do
{
while (--j);
} while (--i);
}
/*舵機方波延時向小車右方*/
void Delay600us() //@11.0592MHz
{
unsigned char i, j;
_nop_();
i = 2;
j = 15;
do
{
while (--j);
} while (--i);
}
void Delay19400us() //@11.0592MHz
{
unsigned char i, j;
_nop_();
i = 35;
j = 197;
do
{
while (--j);
} while (--i);
}
/*舵機方波延時朝向小車左方*/
void Delay17500us() //@11.0592MHz
{
unsigned char i, j;
i = 32;
j = 93;
do
{
while (--j);
} while (--i);
}
void Delay2500us() //@11.0592MHz
{
unsigned char i, j;
i = 5;
j = 120;
do
{
while (--j);
} while (--i);
}
/*********************************************
QX_A11智能小車PWM輸出
入口參數(shù)：無
出口參數(shù): 無
說明：裝載PWM輸出,pwm_left,pwm_right分別配置左右輸出高電平時間，請設置全局對應行走方向（x，y）數(shù)據(jù)一致調(diào)速
**********************************************/
void LoadPWM(void)
{
if(pwm_left > PWM_HIGH_MAX) pwm_left = PWM_HIGH_MAX; //如果左輸出寫入大于最大占空比數(shù)據(jù)，則強制為最大占空比。
if(pwm_left < PWM_HIGH_MIN) pwm_left = PWM_HIGH_MIN; //如果左輸出寫入小于最小占空比數(shù)據(jù)，則強制為最小占空比。
if(pwm_right > PWM_HIGH_MAX) pwm_right = PWM_HIGH_MAX; //如果右輸出寫入大于最大占空比數(shù)據(jù)，則強制為最大占空比。
if(pwm_right < PWM_HIGH_MIN) pwm_right = PWM_HIGH_MIN; //如果右輸出寫入小于最小占空比數(shù)據(jù)，則強制為最小占空比。
if(pwm_val_left<=pwm_left) Left_moto_pwm = 1; //裝載左PWM輸出高電平時間
else Left_moto_pwm = 0; //裝載左PWM輸出低電平時間
if(pwm_val_left>=PWM_DUTY) pwm_val_left = 0; //如果左對比值大于等于最大占空比數(shù)據(jù)，則為零
if(pwm_val_right<=pwm_right) Right_moto_pwm = 1; //裝載右PWM輸出高電平時間
else Right_moto_pwm = 0; //裝載右PWM輸出低電平時間
if(pwm_val_right>=PWM_DUTY) pwm_val_right = 0; //如果右對比值大于等于最大占空比數(shù)據(jù)，則為零
}
//冒泡排序
void QXMBOT_bubble(unsigned int *a,unsigned char n) /*定義兩個參數(shù)：數(shù)組首地址與數(shù)組大小*/
{
unsigned int i,j,temp;
for(i = 0;i < n-1; i++)
{
for(j = i + 1; j < n; j++) /*注意循環(huán)的上下限*/
{
if(a[i] > a[j])
{
temp = a[i];
a[i] = a[j];
a[j] = temp;
}
}
}
}
/*====================================
函數(shù)名：QXMBOT_RefreshDistance
參數(shù) ：無
返回值：經(jīng)過冒泡排序后的距離
描述：經(jīng)過5次測距，去除最大值和最小值，取中間3次平均值
距離單位：毫米
====================================*/
unsigned int QXMBOT_RefreshDistance()
{
unsigned char num;
unsigned int Dist;
for(num=0; num<5; num++)
{
DistBuf[num] = QXMBOT_GetDistance();
Delay_Ms(60);//測距周期不低于60毫秒
}
QXMBOT_bubble(DistBuf, 5);//
Dist = (DistBuf[1]+DistBuf[2]+DistBuf[3])/3; //去掉最大和最小取中間平均值
return(Dist);
}
/*超聲波觸發(fā)*/
void QXMBOT_TrigUltrasonic()
{
TrigPin = 0; //超聲波模塊Trig 控制端
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
TrigPin = 1; //超聲波模塊Trig 控制端
_nop_();_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();_nop_();
TrigPin = 0; //超聲波模塊Trig 控制端
}
/*====================================
函數(shù)名：QXMBOT_GetDistance
參數(shù) ：無
返回值：獲取距離單位毫米
描述：超聲波測距
通過發(fā)射信號到收到回響信號的時間測試距離
單片機晶振11.0592Mhz
注意測距周期為60ms以上
====================================*/
unsigned int QXMBOT_GetDistance()
{
unsigned int Distance = 0; //超聲波距離
unsigned int Time=0; //用于存放定時器時間值
QXMBOT_TrigUltrasonic(); //超聲波觸發(fā)
while(!EchoPin); //判斷回響信號是否為低電平
Timer1On; //啟動定時器1
while(EchoPin); //等待收到回響信號
Timer1Off; //關閉定時器1
Time=TH1*256+TL1; //讀取時間
TH1=0;
TL1=0; //清空定時器
Distance = (float)(Time*1.085)*0.17;//算出來是MM（一般超聲測距公式、12mHZ的芯片是=Time*0.17）如果用0.017/算出來是cM，那么后面數(shù)據(jù)對應要改
return(Distance);//返回距離
}
/*====================================
函數(shù)名：QXMBOT_PTZ_Avoid
參數(shù) ：val設置避障觸發(fā)距離
返回值：無
描述：智能小車舵機云臺避障
距離單位：毫米
====================================*/
void QXMBOT_PTZ_Avoid(unsigned int val)
{
unsigned int Dis;//距離暫存變量
Dis = QXMBOT_GetDistance();//獲取超聲波測距距離,單位：毫米
if((Dis > 30) && (Dis < val))// 設置避障觸發(fā)距離
{
LED1=0,LED2=0,beep=1;//LED1,2點亮，鳴笛
stop(); //停車
Delay_Ms(60);
LED1=1,LED2=1,beep=1;//LED1,2熄滅，靜音
/*舵機左轉(zhuǎn)測距*/
QXMBOT_ServoLeft();
LeftDistance = QXMBOT_RefreshDistance();
/*舵機右轉(zhuǎn)測距*/
QXMBOT_ServoRight();
RightDistance = QXMBOT_RefreshDistance();
/*舵機正前方測距*/
QXMBOT_ServoFront();
FrontDistance = QXMBOT_RefreshDistance();
if((FrontDistance<100) && (LeftDistance<100) && (RightDistance<100))
{
do{
left_run(180, 180);//原地左轉(zhuǎn)，電機動力值
Delay_Ms(100);//電機轉(zhuǎn)動時間
/*舵機正前方測距*/
QXMBOT_ServoFront();
Dis = QXMBOT_RefreshDistance();
}while(Dis < 100);//循環(huán)、距離
}else if(FrontDistance<60)
{
LED1=0,LED2=0;//LED1亮,2點亮
back_run(50, 50);//后退
Delay_Ms(100);
LED1=1,LED2=1;//LED1滅,2滅
}else if((FrontDistance>LeftDistance) && (FrontDistance>RightDistance))
{
stop(); //停車
Delay_Ms(60);
forward(20, 50);//前進
}else if(LeftDistance > RightDistance)
{
LED1=1,LED2=0;//LED1滅,2點亮
stop(); //停車
Delay_Ms(60);
left_run(180, 180);//原地左轉(zhuǎn)，調(diào)速動力值。
Delay_Ms(100); //轉(zhuǎn)動時間，翻倍更改才比較明顯。
LED1=1,LED2=1;//LED1滅,2點滅
}else if(RightDistance > LeftDistance)
{
LED1=0,LED2=1;//LED1亮,2點滅
stop(); //停車
Delay_Ms(60);
right_run(190, 190);//原地右轉(zhuǎn)，調(diào)速動力值。
Delay_Ms(100);//轉(zhuǎn)動時間，翻倍更改才比較明顯。
LED1=1,LED2=1;//LED1滅,2滅
}
}
else
{
forward(20, 50);//前進
Delay_Ms(60);
}
}
/*=================================================
*函數(shù)名稱：QXMBOT_ServoFront
*函數(shù)功能：云臺向前轉(zhuǎn)動
*調(diào)用：
*輸入：
=================================================*/
void QXMBOT_ServoFront()
{
char i;
EA_off; //關閉中斷否則會影響舵機轉(zhuǎn)向
for(i=0;i<10;i++)
{
ServoPin = 1;
Delay1550us();
ServoPin = 0;
Delay18450us();
}
EA_on; //開中斷
Delay_Ms(100);
}
/*=================================================
*函數(shù)名稱：QXMBOT_ServoLeft
*函數(shù)功能：云臺向左轉(zhuǎn)動
*調(diào)用：
*輸入：
=================================================*/
void QXMBOT_ServoLeft()
{
char i;
EA_off; //關閉中斷否則會影響舵機轉(zhuǎn)向
for(i=0;i<10;i++)
{
ServoPin = 1;
Delay2500us();
ServoPin = 0;
Delay17500us();
}
EA_on; //開中斷
Delay_Ms(100);
}
/*=================================================
*函數(shù)名稱：QXMBOT_ServoFront
*函數(shù)功能：云臺向右轉(zhuǎn)動
*調(diào)用：
*輸入：
=================================================*/
void QXMBOT_ServoRight()
{
char i;
EA_off; //關閉中斷否則會影響舵機轉(zhuǎn)向
for(i=0;i<10;i++)
{
ServoPin = 1;
Delay600us();
ServoPin = 0;
Delay19400us();
}
EA_on; //開中斷
Delay_Ms(100);
}
/********************* Timer0初始化************************/
void Timer0_Init(void)
{
TMOD |= 0x02;//定時器0，8位自動重裝模塊
TH0 = 164;
TL0 = 164;//11.0592M晶振，12T溢出時間約等于100微秒
TR0 = 1;//啟動定時器0
ET0 = 1;//允許定時器0中斷
}
/*定時器1初始化*/
void Timer1_Init(void)
{
TMOD |= 0x10; //定時器1工作模式1,16位定時模式。T1用測ECH0脈沖長度
TH1 = 0;
TL1 = 0;
ET1 = 1; //允許T1中斷
}
/********************* Timer0中斷函數(shù)************************/
void timer0_int (void) interrupt 1
{
pwm_val_left++;
pwm_val_right++;
LoadPWM();//裝載PWM輸出
}
/* Timer0 interrupt routine */
void tm1_isr() interrupt 3 using 1
{
Timer1Overflow = 1; //計數(shù)器1溢出標志位
EchoPin = 0; //超聲波接收端
}