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)
{
	DriveLeftMo1 = 0;
	DriveLeftMo2 = 1;
	P1 = 0x7F;
/*
	DriveRightMo1 = 0 ;
	DriveRightMo2 = 1 ;
/*	_nop_();
	Delay(1);  //.25ms
	_nop_();
	initial();
	_nop_();
 /*
 //	start up 
	Stop();	
	MotorLeft(1,1);
	MotorRight(1,1);
	Delay(800);
	MotorLeft(3,1);
	MotorRight(3,1);
	Delay(800);	
	while(1)
	{
		MotorLeft(26,1);
		MotorRight(28,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_ ();
   		}
  	}
}