twoscoremid.c

蛇形机器人程序

/*
	Put the cube from right side of zone to two-score zone
*/

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

//FollowLineTime Control
unsigned int iTurnTime = 0;
extern unsigned int iFollowLineTime;
#define TwoSensorMid  5000				 //about 1000 = 1s	 2000
//
extern const unsigned char LoopLostLine1; 
extern const unsigned char LoopLostLine2;
unsigned char iMidSearchTime = 0;

//~~~~~~Function Delaration~~~~~~~~~~~~~~~
extern void Delay(unsigned int time);
extern void FollowLine(char Speed);
extern void SensorSta(void);
extern void MotorLeft(char Speed,bit Direction);
extern void MotorRight(char Speed,bit Direction);
extern void Stop(void);
extern void UpdateStatus(void);

void TwoScoreMidRight(char Speed);
void TwoScoreMidRightBack(char Speed);
void TwoScoreLeft(char Speed);
void TwoScoreRight(char Speed);
//~~~~~~~~~~~~~~~~~~~~~~~~
void TwoScoreMidRight(char Speed)
		// from right side to Two Score Zone 
{
		while(iFollowLineTime < 1500)
	{
		SensorSta();
		FollowLine((Speed+1));			
		iFollowLineTime++;				  
	}
	iFollowLineTime = 0;
/*
	MotorLeft((Speed-3),1);
	_nop_();		
	MotorRight((Speed-3),1);
	_nop_();
	Delay(17500);	
 */
	MotorLeft((Speed+1),1);
	_nop_();		
	MotorRight((Speed+3),1);
	_nop_();
	Delay(20000);

	//leaing left
	MotorLeft((Speed-2),1);			// in the current version moving forward
	_nop_();				// in the reality 0
	MotorRight((Speed+2),1);
	_nop_();
	Delay(35000);

	
	while((HozSensor1) || (MidSensor1))
	{
	 	MotorLeft((Speed-3),1);			// in the current version moving forward
		_nop_();				// in the reality 0
		MotorRight((Speed-1),1);
		_nop_();
		//SensorSta();
	}


	if( !( (HozSensor1) && (MidSensor2) &&
			(MidSensor1)))
	{
		Delay(7500);
		
		Stop();	
		Delay(1000);
		Stop();
		Delay(50000);	//for test
		Delay(50000);	//for test
		while(HozSensor2 == 1) 	// left hoz sensor 
								//use driect Hozsensor flag
		{
			MotorLeft(5,1);		//deccerate		   3
			_nop_ ();
			MotorRight(5,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(3,1);		//left: speed 2 forward
			_nop_ ();
			MotorRight(3,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(500);
	while(//(iTurnTime < 1000)&&
				(MidSensor1 == 1)||(MidSensor2 == 1))
	{
		MotorLeft(5,0);
		_nop_();
		MotorRight(5,1);
		_nop_();
		iTurnTime++;
	}
	iTurnTime = 0;

	Stop();
	
		Delay(50000);	//for test
		Delay(50000);	//for test
	}
	
	MotorLeft((Speed-2),1);
	_nop_();		
	MotorRight((Speed-2),1);
	_nop_();
	Delay(5000);

	while(iFollowLineTime < TwoSensorMid)
	{
		SensorSta();
		FollowLineMid((Speed-2));			
		iFollowLineTime++;				  
	}
	iFollowLineTime = 0;

}

void TwoScoreLeft(char Speed)
{
	MotorLeft(Speed,1);
	MotorRight(Speed,1);
	Delay(50000);
	Delay(50000);
	Delay(50000);
	Stop();
	Delay(50000);

	MotorLeft((Speed+1),1);
	MotorRight((Speed),1);
	Delay(10000);

	MotorLeft(Speed,1);
	MotorRight(Speed,1);
	Delay(50000);

	Stop();
	Delay(50000);	//for test
	
	while((HozSensor1) || (MidSensor1)||(MidSensor2)||(HozSensor2))
	{
	 	MotorLeft((Speed-1),1);			// in the current version moving forward
		_nop_();				// in the reality 0
		MotorRight((Speed-2),1);
		_nop_();
		//SensorSta();
		Stop();
		Delay(50000);	//for test

	}
	if( !( (HozSensor1) && (MidSensor2) &&
			(MidSensor1)))
	{
		Delay(7500);
		
		Stop();	
		Delay(1000);
		while(HozSensor2 == 1) 	// left hoz sensor 
								//use driect Hozsensor flag
		{
			MotorLeft(5,1);		//deccerate		   3
			_nop_ ();
			MotorRight(5,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(3,1);		//left: speed 2 forward
			_nop_ ();
			MotorRight(3,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();			//
		}
		ImpulseLeft();
	
	}
	while(iFollowLineTime < TwoSensorMid)
	{
		SensorSta();
		FollowLine((Speed-2));			
		iFollowLineTime++;				  
	}
	iFollowLineTime = 0;

}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void TwoScoreMidRightBack(char Speed)
		// from right side to Two Score Zone using back sensor
{
		while(iFollowLineTime < 1500)
	{
		SensorSta();
		FollowLineBack((Speed+1));			
		iFollowLineTime++;				  
	}
	iFollowLineTime = 0;

	MotorLeft((Speed+3),0);
	_nop_();		
	MotorRight((Speed+1),0);
	_nop_();
	Delay(20000);

	//leaing left
	MotorLeft((Speed+2),0);			// in the current version moving forward
	_nop_();				// in the reality 0
	MotorRight((Speed-2),0);
	_nop_();
	Delay(35000);

	
	while((LineSensor1) || (BackSensor1))
	{
	 	MotorLeft((Speed-1),0);			// in the current version moving forward
		_nop_();				// in the reality 0
		MotorRight((Speed-3),0);
		_nop_();
	}


	if( !( (LineSensor1) && (BackSensor2) &&
			(BackSensor1)))
	{
		Delay(7500);
		
		Stop();	
		Delay(1000);
		Stop();
		Delay(50000);	//for test
		Delay(50000);	//for test
		while(LineSensor2 == 1) 	// left hoz sensor 
								//use driect Hozsensor flag
		{
			MotorLeft(5,0);		//deccerate		   3
			_nop_ ();
			MotorRight(5,1);	//Right: Speed 3 backward
			_nop_ ();
	
			UpdateStatus();			// prepare for FollowLine
		}
		while(BackSensor2 == 1) 	//use driect sensor flag
		{
			MotorLeft(3,0);		//left: speed 2 forward
			_nop_ ();
			MotorRight(3,1);	//Right: Speed 2 backward
			_nop_ ();		
			UpdateStatus();			//
		}
	
	Stop();
	Delay(500);
	while(//(iTurnTime < 1000)&&
				(BackSensor1 == 1)||(BackSensor2 == 1))
	{
		MotorLeft(5,1);
		_nop_();
		MotorRight(5,0);
		_nop_();
		iTurnTime++;
	}
	iTurnTime = 0;

	Stop();
	
		Delay(50000);	//for test
		Delay(50000);	//for test
	}
	
	MotorLeft((Speed-2),0);
	_nop_();		
	MotorRight((Speed-2),0);
	_nop_();
	Delay(5000);

	while(iFollowLineTime < TwoSensorMid)
	{
		SensorSta();
		FollowLineBack((Speed-2));			
		iFollowLineTime++;				  
	}
	iFollowLineTime = 0;

}