catch.c

蛇形机器人程序

#include<reg52.h>
#include<FollowLine.h>
#include<intrins.h>
extern void SensorSta(void);
extern void MotorLeft(char Speed,bit Direction);
extern void MotorRight(char Speed,bit Direction);
extern void Delay(unsigned int time);
extern void Stop(void);
//~~~~~~~~~~~~~~~~~~~~
void Catch(void)
{
 	 while(1)
	 {
	//leaning right using LoopLostLine2
	while((MidSensor2) )
	//leaning right
	{
		MotorLeft(10,0);	//Delta for starting up
		MotorRight(10,1);
		Delay(5);   
													
		MotorLeft(4,0);
		MotorRight(4,1);
		Delay(120);
	}	
	if(!MidSensor2)
	{
						
		MotorLeft(15,1);
		_nop_();
		MotorRight(15,0);	
		_nop_();
		Delay(2000);	
	}
	Stop();
	Delay(50000);
	Delay(50000);
	Delay(50000);
	MotorLeft(4,0);
	MotorRight(4,1);
	Delay(4000);
	}
}