followline.c

蛇形机器人程序

/*
	┍┑   ┍┑	┍┑   ┍┑
	┕┙   ┕┙	┕┙   ┕┙　
		5	  2.5	  5
*/

/*			parameters needed to be adjusted

*/

#include<REG2051.h>
#include<FollowLine.h>
#include<intrins.h>

//~~~~~~~~~~~~~~~~~~~~~
bit DirectionRight;
bit DirectionLeft;

																		  
//~~~~~~~~~~~~~Serial In Sensor time~~~~~~~~
bdata unsigned char LineCurStatus = 0xFF;
// Logic 1 
sbit CurStaMidSensor1 = LineCurStatus^0;	//Mid Sensor1 Current State	   1 is On
sbit CurStaMidSensor2 = LineCurStatus^1;	//Mid Sensor2 Current State	   1 is On
sbit CurStaHozSensor1 = LineCurStatus^2;	//Hoz Sensor Current State	   1 is On 
sbit CurStaHozSensor2 = LineCurStatus^3;	//Hoz Sensor Current State	   1 is On

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
bdata unsigned char LinePreStatus = 0xFF;
sbit PreStaMidSensor1 = LinePreStatus^0;	//Mid Sensor1 Previous State
sbit PreStaMidSensor2 = LinePreStatus^1;	//Mid Sensor2 Previous State
sbit PreStaHozSensor1 = LinePreStatus^2;	//Hoz Sensor Previous State
sbit PreStaHozSensor2 = LinePreStatus^3;	//Hoz Sensor Previous State

//movement status
char PreLeftSpeed = 0;		//Speed Status
char PreRightSpeed = 0;
bit PreLeftDirection = 1 ;
bit PreRightDirection = 1;	

// further adjustment should replace
unsigned char iLostLoopTime = 0;			//current scaning time
unsigned char LoopLostLine1 = 100; 	//Maxmium scaning time flag left
unsigned char LoopLostLine2 = 100; 	//Maxmium scaning time flag	Right
const unsigned char CatchTime1 = 50;		//Maxmiun scaning time for CatchLine
const unsigned char CatchTime2 = 50;

unsigned int iFollowLineTime = 0;			//Parameter to control FollowLineTime

//LineCounter	
unsigned char LineCounterP = 0x00;
unsigned char LineCounterN = 0x00;

bit CounterFlag = 1;						// 0 counter enable
bit BackAdjFlag = 1;						// to forbid lineCounter when adjust moving backward

//~~~~~~~~~~~~~~~~~Function Declaration~~~~~~~~~~~~~~~~~~~~
extern void Delay(unsigned int time);
void MotorLeft(char Speed,bit Direction);
void MotorRight(char Speed,bit Direction);
void FollowLine(char Speed);			// follow normal while line
void Stop(void);

//void TurnLeft(char Speed);
//
void MotorLeft(char Speed,bit Direction)
{
	if((Speed != PreLeftSpeed)||(PreLeftDirection != Direction))	 // 
	{
		DirectionLeft = Direction; //forward
		PreLeftDirection = DirectionLeft;
		LeftSpeed  = Speed;
		PreLeftSpeed = Speed;	//save speed status
		iLeftCounter = 0;
	}
	if (TR0 == 0)		  //open T0 is necessary
	{TR0 = 1;}
	else
	{}

}
void MotorRight(char Speed,bit Direction)
{
	if((Speed != PreRightSpeed)||(PreRightDirection != Direction))	 // 
	{
		DirectionRight = Direction; //forward
		PreRightDirection = DirectionRight;
		RightSpeed = Speed;
		PreRightSpeed = Speed;	//save speed status
		iRightCounter = 0; 
	}
	if (TR0 == 0)
	{TR0 = 1;}
	else
	{}						

}

void Stop(void)
{
	TR0 = 0;
	DriveLeftMo1 = 0;
	DriveLeftMo2 = 0;
	DriveRightMo1 = 0;
	DriveRightMo2 = 0;
 	_nop_();
	_nop_();
	PreLeftSpeed = 0;						  
	PreRightSpeed = 0;
}

//~~~~~~~~~~~~~~~~~~~Standard Follow Line~~~~~~~~~~~~~~~~~~~~~~~
void FollowLine( char Speed)
{
	unsigned char iLostLoopTime = 0;
	iStopStatus= 0;
	if(iStopStatus >= 9000 ) 			//time for move backward
	{
		Stop();
		Delay(1200);
		MotorLeft((Speed-3),0);
		_nop_ ();
		MotorRight((Speed-3),0);
		_nop_ ();
		Delay(10000);
		Stop();
		Delay(1200);
		iStopStatus = 0;
	}
		if ((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 0))
			//Both mid sensors are on line 
			//solve the uncertainty on CROSSING POINT
		{
			MotorLeft((Speed+3),1);
			_nop_ ();
			MotorRight((Speed+3),1);
			_nop_ ();

		//	Delay(Speed*1);
		}
		else if((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 1))
				//left is on  ignore Hozsensor
		{
			MotorLeft((Speed-2),1);	//left leaning more
			_nop_ ();
			MotorRight((Speed+1),1);	 //original value is +2
			_nop_ ();
		//	Delay(Speed*1);
		}
		else if((CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 0))
			//right is on  ignore Hozsensor
		{				
			MotorLeft((Speed+1),1);	//Right leaning more
									//original value is +3
			_nop_ ();
			MotorRight((Speed-2),1);
			_nop_ ();
		//	Delay(Speed*1);
		}
		else // (CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 1)
		{
			BackAdjFlag = 1;
			//Stop();
			if(PreStaMidSensor1 == 1)
			{
				if(PreStaMidSensor2 == 0)
				{
					MotorLeft((Speed+3),1);	   //original value is -2
					_nop_();
					MotorRight((Speed-3),1);   //orignal value is +3
					_nop_();
				}
				else //PreStaMidSensor1 == 1, PreStaMidSensor2 ==1
				{
			/*
					rotate left and right about 30 degree
					parameter 120, 80 ,6200 6300 are carefully selected
					using CurStaMidSensor instead ofMidsenso to capture the line
			*/

					while((MidSensor1)
						&& (iLostLoopTime <= LoopLostLine1))
					// totally lost and loop in the time of LoopLostLine1
					// leaning left	loop time 100
					{	   
						//
						MotorLeft(10,1);
						_nop_();
						MotorRight(10,0);
						_nop_();
						iLostLoopTime++;
						_nop_();
						Delay(100);  		//after adjustment about 20 degree 
											//original value is 141
					}
					if((!MidSensor1))		// Delta impulse to stop
					{
						Stop();
						Delay(1000);
						iLostLoopTime = 0;
					}
					
													// do not find the white line back to the original status
					else	 						 // scaning left false
					{
						MotorLeft(10,0);
						_nop_();
						MotorRight(10,1);
						_nop_();
						Delay(10000);			// after adjustment		orignal value is 8200		
						iLostLoopTime = 0;		// clear the flag
 						
						//leaning right using LoopLostLine2
						while((MidSensor2) && (iLostLoopTime<=LoopLostLine2))
						//leaning right
						{						
							MotorLeft(10,0);
							_nop_();
							MotorRight(10,1);
							_nop_();
							iLostLoopTime++;
							_nop_();
							Delay(110);			 //125
						}
						if((!MidSensor2))		// Delta impulse to stop
						{
							Stop();
							Delay(1000);
							iLostLoopTime = 0;
						}
					
						//~~~~~~~~~~~~~
						else	 // scaning false rotate back to the original position and backward
						{

							MotorLeft(10,1);
							_nop_();
							MotorRight(10,0);
							_nop_();
							Delay(12700);	   // the original value is 6300
								// move back to find the lost position

							Stop();
							Delay(1000);
							MotorLeft((Speed-3),0);
							_nop_();
							MotorRight((Speed-3),0);
							_nop_();

							BackAdjFlag = 0; 	//forbid LineCounter
							_nop_();
							Delay(7700);
							Stop();
							Delay(1000);
							
						}
					 }
			   	}
				
			}
			else //PreStaMidSensor1 == 0, PreStaMidSensor2 ==1
			{
				MotorLeft((Speed-3),1);			//orignal value is +3
				_nop_();
				MotorRight((Speed+3),0);		 //orignal value is -2
				_nop_();
			}
			BackAdjFlag = 1;
			_nop_();

		}
	//	Delay(40); // parameter 1 need further adjustment	
		// LineCounter
	if ((!HozSensor1)	&& (!HozSensor2) &&		  // the original is CurStaHozSensor
		 (!CounterFlag)		&&		
		 (BackAdjFlag))
	{
		CounterFlag = 1;
		LineCounterP++;
	}

	if ((HozSensor1)	&& (HozSensor2) &&
		 (CounterFlag))
	{
		CounterFlag = 0;	// reset to enable counter
	}
		
}