followline_bak.c

蛇形机器人程序

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

#include<stdio.h>
#include<reg52.h>
#include<FollowLine.h>
#include<intrins.h>

//~~~~~~~~~~~~~~~~~~~~~

																		  
//~~~~~~~~~~~~~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



//~~~~~~Delay for let the rotate origin is on the line Cross Point~~~~~~
const unsigned char LineCrossDelay = 1;
		// further adjustment should replace
unsigned char LineCounter = 0x00;

//~~~~~~~~~~~~~~~~~Function Declaration~~~~~~~~~~~~~~~~~~~~
void Delay(unsigned int time);
extern void initial(void);
void MotorLeft(unsigned char Speed, bit Direction);
void MotorRight(unsigned char Speed, bit Direction);
void FollowLine(unsigned char Speed);			// follow normal while line
//void TurnLeft(unsigned char speed);			// For further test
//void TurnRight(unsigned char speed);
void Stop(void);
extern void UpdateStatus(void);	//update all sensor
extern void SensorSta();
//

/*
	Data For PWM
*/

bit DirectionRight;
bit DirectionLeft;

//***************************************************
//~~~~~~~~~~~~~~~~Main function~~~~~~~~~~~~~~~~~~~~~*
//***************************************************
void main (void)
{
	_nop_();
	Delay(1);  //.25ms
	_nop_();
	initial();
	_nop_();
	MotorLeft(3,1);
	_nop_();
	MotorRight(3,1);
	Delay(100);
	MotorLeft(3,0);
	_nop_();
	MotorRight(3,0);
	Delay(100);
	 _nop_();
	while(1)
	{	
		SensorSta();
		FollowLine(4);
	}
}
//~~~~~~~~~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_ ();
   		}
  	}
} 

//~~~~~~~~~~~~~Set Motor Speed and Dirction~~~~~~~~~~~~~~~~~~~~~~~~
void MotorLeft(unsigned char Speed, bit Direction)
{
 
	_nop_();
	_nop_();
	PWM_0 = pwm[Speed];	
	DirectionLeft =Direction;
    DriveLeftMo1 = 0;
    DriveLeftMo2 =0;
	ET0 = 1;  //Timer 0 open
    TR0 = 1; //Timer 0 begins
	_nop_();
	_nop_();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~
 void MotorRight(unsigned char Speed, bit Direction)
{
	_nop_();
	_nop_();
	PWM_1 = pwm[Speed];
	DirectionRight =Direction;
	DriveRightMo1 =0;
	DriveRightMo2 =0; 
	ET0 = 1;  //Timer 0 open
    TR0 = 1; //Timer 0 begins
	_nop_();
	_nop_();
}


//~~~~~~~~~~~~~~~~~~~Standard Follow Line~~~~~~~~~~~~~~~~~~~~~~~
void FollowLine(unsigned char Speed)
{
	unsigned char iLostLoopTime = 0;
//	MotorLeft(Speed,1);
//	MotorRight(Speed,1);

	if(  (CurStaHozSensor1 ==1)&&(CurStaHozSensor2 ==1) )
	{
		//Horizontal Sensors show NOT on line
		//further judgement needs Middle sensor to do 
		if ((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 0))
			//Both mid sensors are on line 
			//solve the uncertainty on CROSSING POINT
		{
			MotorLeft(Speed+0,1);
			_nop_ ();
			MotorRight(Speed+0,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+2,1);	 //original value is +3
			_nop_ ();
		//	Delay(Speed*1);
		}
		else if((CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 0))
			//right is on  ignore Hozsensor
		{
			MotorLeft(Speed+2,1);	//Right leaning more
									//original value is +3
			_nop_ ();
			MotorRight(Speed-2,1);
			_nop_ ();
		//	Delay(Speed*1);
		}
		else // (CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 1)
		{
			//Stop();
			if(PreStaMidSensor1 == 1)
			{
				if(PreStaMidSensor2 == 0)
				{
					MotorLeft(Speed-2,1);
					_nop_();
					MotorRight(Speed+3,1);
					_nop_();
				}
				else //PreStaMidSensor1 == 1, PreStaMidSensor2 ==1
				{
					MotorLeft(Speed-3,1);
					_nop_();
					MotorRight(Speed-3,1);
					_nop_();	
				}
			}
			else //PreStaMidSensor1 == 0, PreStaMidSensor2 ==1
			{
				MotorLeft(Speed+3,1);
				_nop_();
				MotorRight(Speed-2,1);
				_nop_();
			}
/*			while((MidSensor1) && (MidSensor2))	
					// totally lost and loop in the time of LoopLostLine1
					// leaning left
			{
				MotorLeft(Speed-4,1);
				_nop_();
				MotorRight(Speed-4,0);	//Right motor rotate backward
				Delay(Speed);
			}
				// do not find the white line back to the original status
				MotorLeft(Speed-3,0);
				_nop_();
				MotorRight(Speed-2,1);
				_nop_();
				Delay(Speed);
				MotorLeft(Speed-1,0);		
				_nop_();
				MotorRight(Speed-1,0);
				_nop_();
				// clear the flag
				//leaning right using LoopLostLine2
			while((MidSensor1) && (MidSensor2)) 
			{
				MotorLeft(Speed-4,0);
				MotorRight(Speed-4,1);
				Delay(Speed);
			}
			*/			
		}

	}
	else if((CurStaHozSensor1 == 0)&&(CurStaHozSensor2 == 1)) 
	//Hoz sensor show left leaning
	//further adjustment should be added
	{
		if ((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 0))
			//Both mid sensors are on line 
			//solve the uncertainty on CROSSING POINT
		{
			MotorLeft(Speed-0,1);
			_nop_ ();
			MotorRight(Speed+0,1);
			_nop_ ();
			//Delay(Speed*1);
		}
		else if((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 1))
			//left is on  ignore Hozsensor
		{
		 	MotorLeft(Speed-2,0);	//left leaning more
			_nop_ ();
			MotorRight(Speed+3,1);
			_nop_ ();
			//Delay(Speed*1);
		}
		else if((CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 0))
				//right is on  ignore Hozsensor
		{
			MotorLeft(Speed+2,1);	//Right leaning more  //the original value is +3
			_nop_ ();
			MotorRight(Speed-2,0);
			_nop_ ();
			
		}
		else // (CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 1)
		{
			//Stop();
			if(PreStaMidSensor1 == 1)
			{
				if(PreStaMidSensor2 == 0)
				{
					MotorLeft(Speed-2,0);
					_nop_();
					MotorRight(Speed+3,1);
					_nop_();
				}
				else //PreStaMidSensor1 == 1, PreStaMidSensor2 ==1
				{
					MotorLeft(Speed-3,1);
					_nop_();
					MotorRight(Speed-3,1);
					_nop_();	
				}
			}
			else //PreStaMidSensor1 == 0, PreStaMidSensor2 ==1
			{
				MotorLeft(Speed+3,1);
				_nop_();
				MotorRight(Speed-2,0);
				_nop_();
			}
/*			while((MidSensor1) && (MidSensor2))	// && (iLostLoopTime<=LoopLostLine1))
					// totally lost and loop in the time of LoopLostLine1
					// leaning left
			{
				MotorLeft(Speed-3,1);
				_nop_();
				MotorRight(Speed-4,0);
				Delay(Speed*1);
			}
				// do not find the white line back to the original status
				MotorLeft(Speed-3,0);
				_nop_();
				MotorRight(Speed-2,1);
				_nop_();
				Delay(Speed*1);
				MotorLeft(Speed-1,0);
				_nop_();
				MotorRight(Speed-1,0);
				_nop_();
				// clear the flag
				iLostLoopTime = 0;
				//leaning right using LoopLostLine2
			while((MidSensor1) && (MidSensor2))  //&& (iLostLoopTime<=LoopLostLine2))
			{
				MotorLeft(Speed-4,0);
				MotorRight(Speed-4,1);
				Delay(Speed*1);
			}
			*/
		}

	}
	else if((CurStaHozSensor1 == 1)&&(CurStaHozSensor2 == 0)) 
	//Hoz sensors show right leaning
	//further adjustment should be added
	{
		if ((CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 1))
			//Both mid sensors are on line