智能小车双舵机避障4WD小车单片机程序舵机转向智能小车 -

/*********************************************************************************
* 【版本】： 1.1
* 【实验平台】： QX-A11智能小车 STC89C52
* 【外部晶振】： 11.0592mhz
* 【主控芯片】： STC89C52
* 【编译环境】： Keil μVisio4
* Copyright(C) QXMBOT
* All rights reserved
***********************************************************************************
* 【程序功能】：QX-A11舵机云台避障参考程序
* 【注意事项】：避免小车轮子堵转
**********************************************************************************/
#include <reg52.h>//51头文件
#include <QX_A11.h>//QX_A11智能小车配置文件
#include <intrins.h>
bit Timer1Overflow; //计数器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的周期，数值为定时器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);//毫秒级延时函数
void forward(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小车前进
void left_run(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小车左转
void right_run(unsigned char LeftSpeed,unsigned char RightSpeed);//QX_A11智能小车右转
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);//舵机左转
void QXMBOT_ServoRight(void);//舵机右转
void Servo1front(void);//舵机B向前
void Servo1left(void);//舵机B左转
void Servo1right(void);//舵机B右转
void QXMBOT_PTZ_Avoid(unsigned int val);//舵机云台避障
unsigned int QXMBOT_GetDistance(void);//获取超声波距离
void QXMBOT_TrigUltrasonic(void);// 触发超声波
unsigned int QXMBOT_RefreshDistance(void);//超声波测距
/*主函数*/
void main(void)
{
Timer0_Init();//定时0初始化
Timer1_Init();//定时0初始化
EA_on; //开中断
QXMBOT_ServoFront();//舵机向前
Servo1front(); //B舵机向前
Delay_Ms(1000);
forward(20,20);//前进5v速度，7.4v情况下forward全部调成（80,80）可调速。
while(1)
{
QXMBOT_PTZ_Avoid(500);//舵机云台避障，LCD1602显示距离,设置触发距离300毫米
}
}
/*********************************************
小车前进
入口参数：LeftSpeed，RightSpeed
说明：LeftSpeed，RightSpeed分别设置左右车轮转速
**********************************************/
void forward(unsigned char LeftSpeed,unsigned char RightSpeed)//不能放1号舵机参数Servo1front(),动力抖颤.
{
pwm_left = LeftSpeed,pwm_right = RightSpeed;//设置速度
left_motor_go; //左电机前进
right_motor_go; //右电机前进
}
/*********************************************
小车左转，入口参数：LeftSpeed，RightSpeed
说明：LeftSpeed，RightSpeed分别设置左右车轮转速
**********************************************/
void left_run(unsigned char LeftSpeed,unsigned char RightSpeed)
{
pwm_left = LeftSpeed,pwm_right = RightSpeed;//设置速度
left_motor_go; //左电机后退
right_motor_go; //右电机前进
}
/*********************************************
小车右转，入口参数：LeftSpeed，RightSpeed
说明：LeftSpeed，RightSpeed分别设置左右车轮转速
**********************************************/
void right_run(unsigned char LeftSpeed,unsigned char RightSpeed)
{
pwm_left = LeftSpeed,pwm_right = RightSpeed;//设置速度
right_motor_go;//右电机后退
left_motor_go; //左电机前进
}
/*********************************************
小车后退
入口参数：LeftSpeed，RightSpeed
说明：LeftSpeed，RightSpeed分别设置左右车轮转速
**********************************************/
void back_run(unsigned char LeftSpeed,unsigned char RightSpeed)
{
pwm_left = LeftSpeed,pwm_right = RightSpeed;//设置速度
right_motor_back;//右电机后退
left_motor_back; //左电机后退
}
/*********************************************
小车停车
入口参数：无
说明：小车停车
**********************************************/
void stop(void)
{
left_motor_stops;
right_motor_stops;
}
/*====================================
函数：void Delay_Ms(INT16U ms)
参数：ms，毫秒延时形参
描述：12T 51单片机自适应主时钟毫秒级延时函数
====================================*/
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输出
入口参数：无
出口参数: 无
说明：装载PWM输出,如果设置全局变量pwm_left,pwm_right分别配置左右输出高电平时间
**********************************************/
void LoadPWM(void)
{
if(pwm_left > PWM_HIGH_MAX) pwm_left = PWM_HIGH_MAX; //如果左输出写入大于最大占空比数据，则强制为最大占空比。
if(pwm_left < PWM_HIGH_MIN) pwm_left = PWM_HIGH_MIN; //如果左输出写入小于最小占空比数据，则强制为最小占空比。
if(pwm_right > PWM_HIGH_MAX) pwm_right = PWM_HIGH_MAX; //如果右输出写入大于最大占空比数据，则强制为最大占空比。
if(pwm_right < PWM_HIGH_MIN) pwm_right = PWM_HIGH_MIN; //如果右输出写入小于最小占空比数据，则强制为最小占空比。
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; //如果左对比值大于等于最大占空比数据，则为零
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; //如果右对比值大于等于最大占空比数据，则为零
}
//冒泡排序
void QXMBOT_bubble(unsigned int *a,unsigned char n) /*定义两个参数：数组首地址与数组大小*/
{
unsigned int i,j,temp;
for(i = 0;i < n-1; i++)
{
for(j = i + 1; j < n; j++) /*注意循环的上下限*/
{
if(a[i] > a[j])
{
temp = a[i];
a[i] = a[j];
a[j] = temp;
}
}
}
}
/*====================================
函数名：QXMBOT_RefreshDistance距离单位：毫米
参数：无
返回值：经过冒泡排序后的距离
描述：经过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);
}
/*超声波触发*/
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 控制端
}
/*====================================
函数名：QXMBOT_GetDistance
参数：无
返回值：获取距离单位毫米
描述：超声波测距
通过发射信号到收到回响信号的时间测试距离
单片机晶振11.0592Mhz
注意测距周期为60ms以上
====================================*/
unsigned int QXMBOT_GetDistance()
{
unsigned int Distance = 0; //超声波距离
unsigned int Time=0; //用于存放定时器时间值
QXMBOT_TrigUltrasonic(); //超声波触发
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，那么后面数据对应要改
return(Distance);//返回距离
}
/*====================================
函数名：QXMBOT_PTZ_Avoid
参数：val设置避障触发距离
返回值：无
描述：智能小车舵机云台避障
距离单位：毫米
====================================*/
void QXMBOT_PTZ_Avoid(unsigned int val)
{
unsigned int Dis;//距离暂存变量
Dis = QXMBOT_GetDistance();//获取超声波测距距离,单位：毫米
if((Dis > 50) && (Dis < val))// 设置避障触发距离30~val=300
{
LED1=0,LED2=0,beep=0;//LED1,2点亮，鸣笛
stop(); //停车
Delay_Ms(100);
LED1=1,LED2=1,beep=1;//LED1,2熄灭，静音
/*舵机左转测距*/
QXMBOT_ServoLeft();
LeftDistance = QXMBOT_RefreshDistance();
/*舵机右转测距*/
QXMBOT_ServoRight();
RightDistance = QXMBOT_RefreshDistance();
/*舵机正前方测距*/
QXMBOT_ServoFront();
FrontDistance = QXMBOT_RefreshDistance();
if((FrontDistance<150) && (LeftDistance<150) && (RightDistance<150))
{
do{
Servo1right();
Delay_Ms(60);
left_run(20, 100);//原地左转，电机动力值（0~200）超出200则为高电平全速运行
Delay_Ms(200);//电机转动时间
Servo1front();
/*舵机正前方测距*/
QXMBOT_ServoFront();
Dis = QXMBOT_RefreshDistance();
}while(Dis < 200);//循环、距离
}else if(FrontDistance<100)
{
stop(); //停车
back_run(60, 60);//后退，电机动力值
Delay_Ms(150);
}else if((FrontDistance>LeftDistance) && (FrontDistance>RightDistance))
{
stop(); //停车
Delay_Ms(100);
forward(20, 20);//前进，电机动力值
}else if(LeftDistance > RightDistance)
{
LED1=1,LED2=0;//LED1灭,2点亮
stop(); //停车
Servo1right();
Delay_Ms(60);
left_run(20, 100);//原地左转，电机动力值
Delay_Ms(200);//电机转动时间
Servo1front();
LED1=1,LED2=1;//LED1灭,2点灭
}else if(RightDistance > LeftDistance)
{
LED1=0,LED2=1;//LED1亮,2点灭
stop(); //停车
Servo1left();
Delay_Ms(60);
right_run(100, 20);//原地右转，电机动力值
Delay_Ms(200);//电机转动时间，
LED1=1,LED2=1;//LED1灭,2灭
Delay_Ms(60);
Servo1front();
}
}
else
{
forward(20, 20);//前进(不能放1号舵机参数Servo1front()动力抖颤)
Delay_Ms(60);
}
}
/*=================================================
*函数名称：QXMBOT_ServoFront
*函数功能：云台向前转动
*调用：
*输入：
=================================================*/
void QXMBOT_ServoFront()
{
char i;
EA_off; //关闭中断否则会影响舵机转向
for(i=0;i<10;i++)
{
ServoPin = 1;
Delay1550us();
ServoPin = 0;
Delay18450us();
}
EA_on; //开中断
Delay_Ms(100);
}
/*=================================================
*函数名称：QXMBOT_ServoLeft
*函数功能：云台向左转动
=================================================*/
void QXMBOT_ServoLeft()
{
char i;
EA_off; //关闭中断否则会影响舵机转向
for(i=0;i<10;i++)
{
ServoPin = 1;
Delay2500us();
ServoPin = 0;
Delay17500us();
}
EA_on; //开中断
Delay_Ms(100);
}
/*=================================================
*函数名称：QXMBOT_ServoFront
*函数功能：云台向右转动
=================================================*/
void QXMBOT_ServoRight()
{
char i;
EA_off; //关闭中断否则会影响舵机转向
for(i=0;i<10;i++)
{
ServoPin = 1;
Delay600us();
ServoPin = 0;
Delay19400us();
}
EA_on; //开中断
Delay_Ms(100);
}
/*=================================================
*函数名称：QXMBOT_ServoFront
*函数功能：云台B转动前（90°）
=================================================*/
void Servo1front()//舵机B向前
{
char i;
EA_off; //关闭中断否则会影响舵机转向
for(i=0;i<10;i++)
{
Servo1Pin = 1;
Delay1550us();
Servo1Pin = 0;
Delay18450us();
}
EA_on; //开中断
Delay_Ms(60);
}
/*=================================================
*函数名称：QXMBOT_ServoFront
*函数功能：云台B向左转动
=================================================*/
void Servo1left()//舵机B左转45度
{
char i;
EA_off; //关闭中断否则会影响舵机转向
for(i=0;i<10;i++)
{
Servo1Pin = 1;
Delay_Ms(2);
Servo1Pin = 0;
Delay_Ms(18);
}
EA_on; //开中断
Delay_Ms(60);
}
/*=================================================
*函数名称：QXMBOT_ServoFront
*函数功能：云台B向右转动
=================================================*/
void Servo1right()//舵机B右转45度
{
char i;
EA_off; //关闭中断否则会影响舵机转向
for(i=0;i<10;i++)
{
Servo1Pin = 1;
Delay_Ms(1);
Servo1Pin = 0;
Delay_Ms(19);
}
EA_on; //开中断
Delay_Ms(60);
}
/********************* 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中断函数************************/
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; //计数器1溢出标志位
EchoPin = 0; //超声波接收端
}

复制代码