robot629.asm.c

一个红外避障的六足爬虫机器人程序

#include<reg51.h>
#include<intrins.h>
#define uint unsigned int
#define uchar unsigned char

#define HOME 14
#define BACK 3
#define FOR 25

sbit RAD = ;		//红外发射口
sbit Radio = ;		//红外接收口
sbit Led_L = ;		//左边小灯
sbit Led_R = ;		//右边小灯
sbit SPK = ;		//扬声器
sbit DJH = ;		//头部马达
sbit DJZ = ;		//中间马达
sbit DJL = ;		//左边马达
sbit DJR = ;		//右边马达
uint BLOCK = 0;		//全局变量，供判断前方有无障碍

void delay_10us(uchar n)		//10us延时
{
	do
	{
		_nop_();				//系统延时函数
		_nop_();
		_nop_();
		_nop_();
		_nop_();
	}while(--n);
}


void delay_1ms(uint n)			//1ms延时函数
{
	do
	{
		delay_10us(131);
	}while(--n);
}

void delay_100us(uint n)		//100us延时函数
{
	uint i = 0 ;
	for(i=0;i<n;i++)
		delay_10us(10);
}

void Init_Radio()				//红外线发射 初始化函数
{
	Radio = 0;					//关发射输出
	IE = 0x00;					//关全部中断
	IP = 0x01;					//设置中断优先级
	TMOD = 0x22;				//
	TH1 = 0xf3;					//40kHZ
	TL1 = 0xf3;
	EA = 1;						//开总中断
}

void Inter() interrupt 0		//红外中断
{
	EA = 0;						//关总中断
	BLOCK = 1;					//全局变量置1，表示前方有障碍
	EA = 1;						//开总中断
}

void time_inter1() interrupt 3		//定时器中断T1，产生40kHZ红外发生器
{
	Radio = ~Radio;
}


void Send_Radio()						//发射红外函数
{
	uint m;

	Radio = 0;
	
	for(m=3;m>0;m--)			//40kHZ发送1ms，发送3个波
	{
		delay_1ms(1);
		ET1 = 1;
		TR1 = 1;
		delay_1ms(1);
		ET1 = 0;
		TR1 = 0;
		Radio = 0;
	}
}


void DJH_FOR()		//头部电机正转
{
	DJH = 1;
	delay_100us(FOR);	
	DJH = 0;
	delay_100us(200 - FOR);
}

void DJZ_FOR()		//中间电机正转
{
	DJZ = 1;
	delay_100us(FOR);	
	DJZ = 0;
	delay_100us(200 - FOR);
}

void DJL_FOR()		//左边电机正转
{
	DJL = 1;
	delay_100us(FOR);
	DJL = 0;
	delay_100us(200 - FOR);
}

void DJR_FOR()			//右边电机正转
{
	DJR = 1;
	delay_100us(FOR);
	DJR = 0;
	delay_100us(200 - FOR);
}

void DJH_BACK()		//头部电机反转
{
	DJH = 1;
	delay_100us(BACK);
	DJH = 0;
	delay_100us(200 - BACK);
}

void DJZ_BACK()		//中间电机反转
{
	DJZ = 1;
	delay_100us(BACK);
	DJZ = 0;
	delay_100us(200 - BACK);
}

void DJL_BACK()			//左边电机正转
{
	DJL = 1;
	delay_100us(BACK);
	DJL = 0;
	delay_100us(200 - BACK);
}

void DJR_BACK()			//右边电机正转
{
	DJR = 1;
	delay_100us(BACK);
	DJR = 0;
	delay_100us(200 - BACK);
}

void Go_BACK()			//机器人后退
{
	uint i = 0;
	for(i=0;i<10;i++)
		DJL_BACK();
	for(i=0;i<10;i++)
		DJR_BACK();
	for(i=0;i<5;i++)
		DJZ_FOR();
	for(i=0;i<10;i++)
		DJR_FOR();
	for(i=0;i<10;i++)
		DJL_FOR();
	for(i=0;i<5;i++)
		DJZ_BACK();
}


void Motor_Ready()			//各电机回中点
{
	DJH = 1;
	DJL = 1;
	DJR = 1;
	DJZ = 1;
	delay_100us(HOME);

	DJH = 0;
	DJL = 0;
	DJR = 0;
	DJZ = 0;
	delay_100us(200 - HOME);
}

void Go_Ready()				//机器人回中点
{
	uint i = 0;
	for(i=0;i<15;i++)
		Motor_Ready();
}


void Go_Strait()			//机器人直走
{
	uint i = 0;
	
	for(i=0;i<5;i++)
		DJZ_FOR();
	for(i=0;i<10;i++)
		DJR_BACK();
	for(i=0;i<10;i++)
		DJL_BACK();
	for(i=0;i<5;i++)
		DJZ_BACK();
	for(i=0;i<10;i++)
		DJL_FOR();
	for(i=0;i<10;i++)
		DJR_FOR();
}

void Go_Left()			//机器人左转
{
	uint i = 0;
	
	Led_L = 1;			//左边灯亮

	for(i=0;i<10;i++)
		DJL_BACK();
	for(i=0;i<5;i++)
		DJZ_FOR();
	for(i=0;i<10;i++)
		DJR_BACK();
	for(i=0;i<10;i++)
		DJL_FOR();
	for(i=0;i<5;i++)
		DJZ_BACK();
	for(i=0;i<10;i++)
		DJR_FOR();

	Led_L = 0;			//左边灯灭
}


void Go_Right()			//机器人右转
{
	uint i = 0;
	
	Led_R = 1;			//右边灯亮

	for(i=0;i<10;i++)
		DJR_BACK();
	for(i=0;i<5;i++)
		DJZ_FOR();
	for(i=0;i<10;i++)
		DJL_BACK();
	for(i=0;i<10;i++)
		DJR_FOR();
	for(i=0;i<5;i++)
		DJZ_BACK();
	for(i=0;i<10;i++)
		DJL_FOR();
		
	Led_R = 0;			//右边灯灭
}

void Neck_Left()			//机器人头部左摆后归位
{
	uint i = 0;
	for(i=0;i<5;i++)
		DJH_FOR();
	for(i=0;i<5;i++)
		DJH_BACK();
}

void Neck_Right()			//机器人头部右摆后归位
{
	uint i = 0;
	for(i=0;i<5;i++)
		DJH_BACK();
	for(i=0;i<5;i++)
		DJH_FOR();
}


void Init()						//初始化
{
	uint i = 0;
	DJH=0;						//关闭马达和喇叭
	DJZ=0;
	DJL=0;
	DJR=0;
	SPK=0;
	
	EA = 1;						//开总中断
	Init_Radio();				//红外发射和接收初始化
	Go_Ready();					//机器人回中点
	
	Neck_Left();					//机器人头部来回摆1下再归位，以示正常
	Neck_Right();

	for(i=0;i<3;i++) 			//小灯闪3下，喇叭响3下，以示程序开始执行
	{
		Led_L = 1;
		Led_R = 1;
		SPK = 1;
		delay_1ms(1000);
	}
}

void main()
{
	Init();					//初始化

	while(1)
	{
		Go_Strait();		//直走
		Send_Radio();		//发红外，并接收
		if(BLOCK == 1)		//若左边有障碍
		{
			BLOCK = 0;		//注意标志位复位

			Neck_Left();	//头部左转
			Send_Radio();		//发红外，并接收
			if(BLOCK == 1)		//若左边有障碍
			{
				BLOCK = 0;		//注意标志位复位

				Neck_Right();		//头部右转
				Send_Radio();		//发红外，并接收

				while(BLOCK == 1)	//若右边有障碍
				{
					BLOCK = 0;		//注意标志位复位
					
					Go_BACK();		//后退几步
					Go_Right();		//右转一个角度，同时右边灯亮
					Send_Radio();		//发红外，并接收
				}//while
			}//if
			else				//左边无障碍
			{
				Go_BACK();		//后退几步
				Go_Left();		//左转一个角度，同时左边灯亮
			}//else

			Go_Ready();			//机器人回中点，即调整好姿势，准备前行
		}//if
	}//while
}