超声波避障小车

/*****************ZYDJ-CSB09A舵机控制*****************************
***平台：ZYDJ-CSB09A舵机控制 + Keil U4 + STC89C52*****************
***程序名称：超声波避障程序+舵机转向摇头**************************
***日期：2022-2-13************************************************
***交流QQ:3064689599**********************************************
***晶振:11.0592MHZ*************************************************
******************************************************************/

#include "reg52.h"
#include "intrins.h"
#include "string.h"
#include "delay.h"

sbit ECHO=P1^0;
sbit TRIG=P1^1;
sbit IN1=P2^5;		
sbit IN2=P2^4;
sbit IN3=P2^3;
sbit IN4=P2^2;
sbit left_motor_pwm=P0^7;
sbit right_motor_pwm=P2^1;
sbit servo_pwm=P2^0;

#define L_back     IN1=0;IN2=1 //向后
#define L_go       IN1=1;IN2=0 //向前
#define L_stop     IN1=0;IN2=0	//停止
#define R_back     IN3=0;IN4=1 //向后
#define R_go  	   IN3=1;IN4=0 //向前
#define R_stop  	 IN3=0;IN4=0 //停止

typedef unsigned char u1;
typedef unsigned int u4;

void Port_Init();
void Ultrasonic();
void Compare();
void Timer0_Init();
void Distance();
u1 overflow=2;
u4 read,send;

u1 buff[]={
0x00,0x00,0x00
};//缓冲区，0：正前方。2：左边。1：右边


/*******************************************************************************************************
主程序
*******************************************************************************************************/
void main()
{
  Timer0_Init();
	left_motor_pwm=1;
	right_motor_pwm=1;
	TRIG=0;
	ECHO=0;
	while(1)
	{
		Ultrasonic();
		Distance();
	}
}

/*******************************************************************************************************
舵机中断0初始化
*******************************************************************************************************/
void Timer0_Init()
{
	TMOD|=0x01;
	EA=1;
	TF0=0;
	ET0=1;
	TR0=1;
	TH0 = 0xFE;
  TL0 = 0x33;	
}
/*******************************************************************************************************
若超声波的距离小于等于40cm，停止小车前进，然后转动舵机，使舵机获取左边与右边的超声波距离
若超声波距离大于40cm，小车继续前进；
*******************************************************************************************************/
void Distance()
{
	if(send<=400)//小于等于40cm
	{
		R_stop;
		L_stop;
		overflow=0;
		Ultrasonic();
		buff[1]=send;//数组第一个缓冲区装载左边的距离
		Delay100ms();
		
		overflow=4;
		Ultrasonic();
		buff[2]=send;//数组第二个缓冲区装载右边的距离
		Delay100ms();
		overflow=2;      //舵机回正
		Delay100ms();
		Compare();   //调用比较程序
	}
	if(send>400)//大于40cm
	{
	left_motor_pwm=1;
	right_motor_pwm=1;
		R_go;
		L_go;
	}
}

/*******************************************************************************************************
比较子程序，收集到的超声波值进行比较
*******************************************************************************************************/
void Compare()
{
	if(buff[1]>buff[2])
	{
		L_back;
		R_back;
		Delay200ms();
		R_back;
		L_go;
		Delay100ms();
		R_go;
		L_go;
		memset(buff,'\0',3);//将缓冲区数组中的三个数值全部清0
	}
	if(buff[2]>=buff[1])
	{
			left_motor_pwm=1;
			right_motor_pwm=1;
			L_back;
			R_back;
			Delay200ms();
			L_back;
			R_go;
			Delay100ms();
			R_go;
			L_go;
		  memset(buff,'\0',3);//将缓冲区数组中的三个数值全部清0
	}
}
/*******************************************************************************************************
超声波初始化，配置为定时器1
*******************************************************************************************************/
void Timer1_Ultrasonic_Init()
{
	TMOD|=0x10;
	TR1=1;
	TH1=0x00;
	TL1=0x00;
}
void Port_Init()
{
	TRIG=0;
	ECHO=1;
	Timer1_Ultrasonic_Init();
}

/*******************************************************************************************************
超声波测距离
*******************************************************************************************************/
void Ultrasonic()
{
	Port_Init();
	TRIG=1;
	Delay10us();
	TRIG=0;
	while(!ECHO);
	TR1=1;
	TH1=0x00;
	TL1=0x00;
	while(ECHO);
	TR1=0;
	read=TH1*256+TL1;
  send=read*0.172;//计算公式
  Delay60ms();
}

/*******************************************************************************************************
中断1，控制舵机
*******************************************************************************************************/
void Timer0_Servo()interrupt 1
{
static u1 cnt;
TH0 = 0xFE;
TL0 = 0x33;	
	cnt++;
		if(cnt==40)//溢出满40次清0
		{
			cnt=0;
		}
		
		if(cnt<=overflow)
		{
			servo_pwm=1;
		}
		else
			servo_pwm=0;
}