main.c

蛇形机器人程序

/*
	Main Prgram
*/

#include<reg52.h>
#include<FollowLine.h>
#include<intrins.h>
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#define CatchBoxTime 2000		// original time is 2000
#define CalibrateLine  2000
#define BackLineTime 2500

bit CloseSensor1Com = 1;
bit CloseSensor2Com = 1;
bit FrontFlag = 0;
bit StopFlag = 0;
bit CrashFlag = 1; 	//touching sensor to the 2 score zone
bit ClampFlag = 0;		// 1 = 

unsigned char iCrashTime = 0;
unsigned int iStopTime = 0;
//~~~~~~~~~~~~~~~~~Function Declaration~~~~~~~~~~~~~~~~~~~~
void Delay(unsigned int time);
extern void initial(void);

//***************************************************
//~~~~~~~~~~~~~~~~Main function~~~~~~~~~~~~~~~~~~~~~*
//***************************************************
void main (void)
{

	_nop_();
	Delay(1);  //.25ms
	_nop_();
	initial();						  
	_nop_();

 //	start up 

 
	MotorLeft(1,1);
	MotorRight(1,1);
	Delay(2200);
	MotorLeft(3,1);
	MotorRight(3,1);
	Delay(2200);
	
	MotorLeft(16,1);
	MotorRight(16,1);
	Delay(2200);	

	MotorLeft(31,1);
	MotorRight(31,1);
	Delay(16000);				// original value is 15000
	Stop();
	Delay(4000);


	MotorLeft(16,1);
	MotorRight(16,0);
	Delay(6300);				 // need further adjustment

	Stop();
	Delay(1000);


	while(CloseSensor1)
	{	
		MotorLeft(28,1);
		MotorRight(28,1);
	}

	MotorLeft(27,1);
	MotorRight(31,1);
	Delay(10000);

	MotorLeft(28,1);
	MotorRight(31,1);
	Delay(10000);

	MotorLeft(30,1);
	MotorRight(31,1);
	Delay(40000);

	iLoop = 0;
	iSensorConfirm= 0;
	while(CloseSensor1Com)
	{
		iLoop++;
		MotorLeft(31,1);
		MotorRight(29,1);		
		if(CloseSensor1)
		{
			Delay(20);
			iSensorConfirm++;
		}
		if(iLoop == 20)
		{									 // filter to avoid noise
			if(iSensorConfirm > 18)
			{
				CloseSensor1Com = 0; 
				iLoop = 0;
			}
			else
			{
			 	CloseSensor1Com = 1; 
				iLoop = 0;
			}
		}

	}
	CloseSensor1Com = 0;
	Delay(1500);
	
	Stop();
	Delay(800);
//catch right position

		
	MotorLeft(16,0);	// turn about 45 degree
	MotorRight(16,1);
	Delay(6900);

	Stop();
	Delay(1000);
	
	MotorLeft(31,1);
	MotorRight(31,1);
	Delay(11300);
	
	while(TouchSensor1 && TouchSensor2)
	{
	    ET1=1;
	    TR1=1;
	 	MotorLeft(31,1);
		MotorRight(31,1);
	}
	MotorLeft(31,1);
	MotorRight(31,1);
	Delay(800);
	 //adjust to fit the bridge edge 
		while((TouchSensor1 || TouchSensor2) && CrashFlag )
	{
		if ((!TouchSensor1)&&(TouchSensor2)	)	 //left touch
		{
			MotorLeft(16,0);
			MotorRight(14,0);				
			Delay(2000);		  			
			Stop();
			Delay(800);
		}
		else if ((TouchSensor1)&&(!TouchSensor2)) //right touch
		{
			MotorLeft(14,0);
			MotorRight(16,0);	
			Delay(2000);
			Stop();
			Delay(800);
		}
		else if ((TouchSensor1)&&(TouchSensor2))  // try to crash
		{
			MotorLeft(31,1);
			MotorRight(31,1);	
			Delay(2500);
			iCrashTime++;
		}
		if(iCrashTime >5 )	   // if crash time is over 3time false to try
		{CrashFlag = 0;}


	}
	Stop();
	ReleaseMotor = 0;		//release the blocker?

	Delay(6000);
	ReleaseMotor = 1;

	Stop();
	while(1);


}


//~~~~~~~~~Delay Function~~~~~~~~~~~~~~~~~~~~~~~
void Delay(unsigned int time) //time /ms
{
	unsigned int i;
  	unsigned char j;
	time *=2;
 	for (i=0;i<time;i++)
 	{
		_nop_ ();
  		for (j=0;j<10;j++)
   		{
			_nop_ ();
   		}
  	}
}