followline.c

蛇形机器人程序

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

/*			parameters needed to be adjusted

*/

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

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

unsigned char iLoop = 0x00;	//loop time control
unsigned char iSensorConfirm = 0x00;	//Status confirm
unsigned int iStopStatus = 0x00;

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

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