turn.c

蛇形机器人程序

/*
	Motor Truning Prgram
*/

#include<REG2051.h>
#include<FollowLine.h>
#include<intrins.h>
//~~~~~~~~~~~~~~~~~~
#define TurnTime 700
//~~~~~~Delay for let the rotate origin is on the line Cross Point~~~~~~
extern unsigned int iStopTime;
extern unsigned int iFollowLineTime;
//~~~~~~~~~~~~~~~~~~
void TrunLeft(char Speed);
void TurnRight(char Speed);



//~~~~~~~~~~~~~~~~~~~~~ Motor Trun Left~~~~~~~~~~~~~~~~~~~~~~~~

void TurnLeft(char Speed)
	// assume the use TurnLeft after a lineCross
{
		Delay(6300);		//delay to on the LineCross	 //4500
		
				
		MotorLeft(9,0);		
		_nop_ ();
		MotorRight(9,1);	
		_nop_ ();
		Delay(5000);		//start up
				
		MotorLeft(6,0);		//left: speed 4 forward
		_nop_ ();
		MotorRight(6,1);	//Right: Speed 4 backward
		_nop_ ();
		Delay(1500);		//about 45 degree

		while(HozSensor1 == 1) 	// left hoz sensor 
								//use driect Hozsensor flag
		{
			MotorLeft(4,0);		//deccerate		   3
			_nop_ ();
			MotorRight(4,1);	//Right: Speed 3 backward
			_nop_ ();
		//pressume the distance between sensor and the rotate origin is 30 cm
		//all 4 sensor have lost the line :CurStaHozSensor1 == 0 Not available		
			UpdateStatus();			// prepare for FollowLine
		}
		while(MidSensor1 == 1) 	//use driect sensor flag
		{
			MotorLeft(3,0);		//left: speed 2 forward
			_nop_ ();
			MotorRight(3,1);	//Right: Speed 2 backward
			_nop_ ();
		//pressume the distance between sensor and the rotate origin is 30 cm
		//all 4 sensor have lost the line :CurStaHozSensor1 == 0 Not available		
			UpdateStatus();			//
		}

		UpdateStatus();
	while(iFollowLineTime < TurnTime)
	{
		SensorSta();
		FollowLine((Speed-2));			
		iFollowLineTime++;				  
	}
	iFollowLineTime = 0;


}


//~~~~~~~~~~~~~~~~~~~~~ Motor Trun Right~~~~~~~~~~~~~~~~~~~~~~~~
void TurnRight(char Speed )
// Trun Right
{
//	while((LineSensor1 == 1)||(LineSensor2 == 1)) //locate the rotate orign
//	{}
//	Stop();
	Delay(6000);
			//rotate right
	MotorLeft(1,0);		
	_nop_ ();
	MotorRight(3,0);	
	_nop_ ();
	Delay(5000);		

	while(HozSensor2 == 1) 	// left hoz sensor 
								//use driect Hozsensor flag
	{
		MotorLeft(3,1);		//deccerate		   3
		_nop_ ();
		MotorRight(3,0);	//Right: Speed 3 backward
		_nop_ ();
		//pressume the distance between sensor and the rotate origin is 30 cm
		//all 4 sensor have lost the line :CurStaHozSensor1 == 0 Not available		
		UpdateStatus();			// prepare for FollowLine
	}
		while(MidSensor2 == 1) 	//use driect sensor flag
		{
			MotorLeft(2,1);		//left: speed 2 forward
			_nop_ ();
			MotorRight(2,0);	//Right: Speed 2 backward
			_nop_ ();
		//pressume the distance between sensor and the rotate origin is 30 cm
		//all 4 sensor have lost the line :CurStaHozSensor1 == 0 Not available		
			UpdateStatus();			//
		}
		Stop();
		Delay(2000);

		UpdateStatus();

}