main.c

蛇形机器人程序

/*
	Main Prgram
*/

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

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

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(800);
	MotorLeft(3,1);
	MotorRight(3,1);
	Delay(800);	

	MotorLeft(31,1);
	MotorRight(31,1);
	Delay(15000);
	Stop();
	Delay(1000);


	MotorLeft(29,1);
	MotorRight(29,0);
	Delay(5300);

	Stop();
	Delay(1000);

	while(!CloseSensor1)
	{	
		MotorLeft(28,1);
		MotorRight(28,1);
	}
	MotorLeft(29,1);
	MotorRight(31,1);
	Delay(60000);

	while(CloseSensor1)
	{
		MotorLeft(29,1);
		MotorRight(31,1);
	}
	Stop();
	while(1);
	
	/*
	while(LineCounterP < 3)	// right side zone
							// move right to out of start up zone
	{
		SensorSta();
		FollowLine(16);			   //origanl  8 
	}
	LineCounterP = 0;
	*/

		

}


//~~~~~~~~~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_ ();
   		}
  	}
}