followline_bak.c

蛇形机器人程序

			//solve the uncertainty on CROSSING POINT
		{
			MotorLeft(Speed+0,1);	 //the original value is +3
			_nop_ ();
			MotorRight(Speed-0,1);
			_nop_ ();
			//Delay(Speed*1);
			}
		else if((CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 0))
		//left is on  ignore Hozsensor
		{
		  	MotorLeft(Speed-2,0);	//left leaning more
			_nop_ ();
			MotorRight(Speed+3,1);	 //the original value is +3
			_nop_ ();
			//Delay(Speed*1);
		}
		else if((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 1))
		//right is on  ignore Hozsensor
		{
			MotorLeft(Speed+2,1);	//Right leaning more
			_nop_ ();
			MotorRight(Speed-2,0);
			_nop_ ();
			//Delay(Speed*1);
		}
		else // (CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 1)
		{
			//Stop();
			if(PreStaMidSensor1 == 1)
			{
				if(PreStaMidSensor2 == 0)
				{
					MotorLeft(Speed-2,0);
					_nop_();
					MotorRight(Speed+3,1);
					_nop_();
				}
				else 
				{
					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_();
			}
			//Delay(Speed*15);
/*			while((MidSensor1) && (MidSensor2))	// && (iLostLoopTime<=LoopLostLine1))
				// totally lost and loop in the time of LoopLostLine1
				// leaning left
			{
				MotorLeft(Speed-4,1);
				_nop_();
				MotorRight(Speed-4,0);
				iLostLoopTime++;
				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_();
			//leaning right using LoopLostLine2
			while((MidSensor1) && (MidSensor2))  //&& (iLostLoopTime<=LoopLostLine2))
			{
				MotorLeft(Speed-4,0);
				MotorRight(Speed-4,1);
				Delay(Speed*1);
			}
*/		}

	}
	else if((CurStaHozSensor1 ==0)&&(CurStaHozSensor2 ==0))
	{  
		//crossing a line !!
		//the further adjustment should be rush to pass the cross
		// so that 
		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);
			//accelerate to pass the Line cross 
			LineCounter++;		//Counter Line 
		//	Delay(Speed*1);
		}
		else if((CurStaMidSensor1 == 0)&&(CurStaMidSensor2 == 1))
				//left is on  ignore Hozsensor
		{
			MotorLeft(Speed-2,0);	//left leaning more
			_nop_ ();
			MotorRight(Speed+2,1);	//the 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
									//the original value is +3
			_nop_ ();
			MotorRight(Speed-2,0);
			_nop_ ();
		//	Delay(Speed*1);
		}
		else // (CurStaMidSensor1 == 1)&&(CurStaMidSensor2 == 1)
		{
			//Stop();
						
			//MotorLeft(Speed+4,1);
			//_nop_ ();
			//MotorRight(Speed+4,1);
			//_nop_ ();
			//Delay(Speed*15);
			//accelerate to pass the Line cross
		if(PreStaMidSensor1 == 1)
			{
				if(PreStaMidSensor2 == 0)
				{
					MotorLeft(Speed-2,0);
					_nop_();
					MotorRight(Speed+3,1);
					_nop_();
				}
				else 
				{
					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_();
			}
			// do not move backward

		}

	}
	
		Delay(4); // parameter 1 need further adjustment		
}
/*

//~~~~~~~~~~~~~~~~~~~~~ Motor Trun Left~~~~~~~~~~~~~~~~~~~~~~~~
void TurnLeft( unsigned char speed )
{
	bit Catch = 1; //Identity whether the sensor catch the right line
  	LineCurStatus = P3; // only for test
	while (CurCross == 0)
	{
		Delay(20);
	}
	// waiting for passing a Line cross in the current state

	Delay(speed *LineCrossDelay);
	// delay to let the truning origin is on the line cross point
	// Need to test the rotate origin and the speed
	Stop();		//may down speed stop for accurate control
				//HozSensor1 = 1 , HozSensor2 = 1,
	MotorLeft(4,1);		//left: speed 4 forward
	_nop_ ();
	MotorRight(0,1);	//Right:stop
	_nop_ ();
	//pressume the distance between sensor and the rotate origin is 30 cm
	//all 4 sensor have lost the line :CurStaHozSensor1 == 0 Not available

	while(CurStaHozSensor2 == 0)
	{
	LinePreStatus = LineCurStatus;
	_nop_ ();
	LineCurStatus = P3; // only for test
	// update statues
	}
	//CurStaHozSensor2 == 1, the sensor has touch the white line
	MotorLeft(2,1);
	_nop_ ();
	MotorRight(0,1); 	//decrease the speed
	_nop_ ();
	Delay(speed*5);	 	// Hold a time to true to the right line
	FollowLine(6);		//the Standard FollowLine Subprogram
	//~~~~~~~~~~~~~~~~~~~~
	while (Catch)
	// swing need right line to comfrom
	{
		LineCurStatus = P3; // only for test
		MotorLeft(0,1);
		_nop_ ();
		MotorRight(2,-1);	//trun right for comfirm catching the right line
		_nop_ ();
		Delay(speed*3);
		if(	(PreStaHozSensor2 == 1) &&
			(CurStaHozSensor2 == 0)	&& //left hoz sensor has passed a line
			(CurStaMidSensor1 == 1 )&&
			(CurStaMidSensor2 == 1)		)
		{Catch = 0;}		// jump out from while()
								//Only if all these conditions valid stop while
		LinePreStatus = LineCurStatus; //Update status
		_nop_ ();
		MotorLeft(2,1);
		MotorRight(1,1);	//trun right for comfirm catching the right line
		Delay(speed*3);		//hold a time to adjust
		if(	(PreStaHozSensor1 == 0) &&
			(CurStaHozSensor1 == 0)	&& // right Hoz sensor never passed a line
			(CurStaMidSensor1 == 1 )&&
			(CurStaMidSensor2 == 1)		)
		{Catch = 0;}
		LinePreStatus = LineCurStatus;
	    _nop_ ();
	}
	LinePreStatus = LineCurStatus;;
	FollowLine(6);

}

//~~~~~~~~~~~~~~~~~~~~~ Motor Trun Right~~~~~~~~~~~~~~~~~~~~~~~~
void TurnRight( unsigned char speed )
// Trun Right
{
	bit Catch = 1; //Identity whether the sensor catch the right line
	LineCurStatus = P3; // only for test
	while (CurCross == 0)	 	
	{
		Delay(10);
		LineCurStatus = P3;
	}
		// wait for passing a Line cross in the current state
	
	Delay(speed *LineCrossDelay);
	// delay to let the truning origin is on the line cross point
	// Need to test the rotate origin and the speed
	Stop();		//may down speed stop for accurate control
				//HozSensor1 = 1 , HozSensor2 = 1,
	MotorLeft(0,1);		//left: stop
	_nop_ ();
	MotorRight(6,1);	//Right:speed 4 forward
	_nop_ ();
	//pressume the distance between sensor and the rotate origin is 30 cm
	//all 4 sensor have lost the line :CurStaHozSensor2 == 0

	while(CurStaHozSensor2 == 0)
	{
	LinePreStatus = LineCurStatus;
	_nop_ ();
	LineCurStatus = P3; // only for test
		// update statues
	}
	//CurStaHozSensor2 == 1, the sensor has touch the white line
	MotorLeft(0,1);
	_nop_ ();
	MotorRight(2,1); 	//decrease the speed
	_nop_ ();
	Delay(speed*5);	 	// Hold a time to true to the right line
	FollowLine(6);		//the Standard FollowLine Subprogram
	//~~~~~~~~~~~~~~~~~~~~
	while (Catch)
	// swing need right line to comfrom
	{
		LineCurStatus = P3; // only for test
		MotorLeft(2,-1);
		_nop_ ();
		MotorRight(0,1);	//trun left for comfirm catching the right line
		_nop_ ();
		Delay(speed*3);
		if(	(PreStaHozSensor1 == 1) &&
			(CurStaHozSensor1 == 0)	&& //left hoz sensor has passed a line
			(CurStaMidSensor1 == 1 )&&
			(CurStaMidSensor2 == 1)		)
		{Catch = 0;}		// jump out from while()
							//Only if all these conditions valid stop while
		LinePreStatus = LineCurStatus; //Update status
		_nop_ ();
		MotorLeft(0,1);
		MotorRight(2,1);	//trun right for comfirm catching the right line
		Delay(speed*3);		//hold a time to adjust
		if(	(PreStaHozSensor2 == 0) &&
			(CurStaHozSensor2 == 0)	&& // right Hoz sensor never passed a line
			(CurStaMidSensor1 == 1 )&&
			(CurStaMidSensor2 == 1)		)
		{Catch = 0;}
		LinePreStatus = LineCurStatus;
		_nop_ ();
	}
	LinePreStatus = LineCurStatus;
	_nop_ ();
	FollowLine(6);
}
*/
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void Stop()
{
	DriveLeftMo1 = 0;
	DriveLeftMo2 = 0;
	DriveRightMo1 = 0;
	DriveRightMo2 = 0;
	TR0 = 0;
	ET0 = 0;
	TF0 = 0;
 	_nop_();
	_nop_();
}