#include"Config.h"
#include"IOCfg.c"
#include"TimeDLY.h"
#include"StepMot.h"
#include"Follow.h"

//尋線函數：Follow()
void  Follow(void)
{
	INT8U temp;
	temp = Follow_L;
	temp = (temp<<1)|Follow_M;
	temp = (temp<<1)|Follow_R;
	switch(temp&0x07)
	{
		//沒檢測到黑線時，原地打轉
		case 0x00:  LMotRun(LSPEED);RMotStop();     break;
		//僅右邊檢測到黑線時，左電機快轉，右電機慢轉
		case 0x01:  LMotRun(HSPEED);RMotRun(LSPEED);break;
		//僅中間檢測到黑線時，正常，左右電機都快轉
		case 0x02:  LMotRun(HSPEED);RMotRun(HSPEED);break;
		//中間以及右邊檢測到黑線時，左電機快轉，右電機慢轉
		case 0x03:  LMotRun(HSPEED);RMotRun(LSPEED);break;
		//僅左邊檢測到黑線時，左電機慢轉，右電機快轉
		case 0x04:  LMotRun(LSPEED);RMotRun(HSPEED);break;
		case 0x05:  break;  //不可能出現此情況
		//中間以及左邊檢測到黑線時，左電機慢轉，右電機快轉
		case 0x06:  LMotRun(LSPEED);RMotRun(HSPEED);break;	
		//中間，左右邊都檢測到黑線時，原地打轉
		case 0x07:  LMotRun(LSPEED);RMotStop();     break;
		default: break;
		}
	}