sensor2.c

来自「蛇形机器人程序」· C语言 代码 · 共 145 行

/*
	sensor new 
	base on slice windows means to avoid twitter
*/
#include<FollowLine.h>
#include<INTRINS.h>
#include<REG2051.h>
const unsigned char LoopTime = 10;
const unsigned char ConfirmTime = 8;
		//confirm 	switch valve value								 	


//
void UpdateStatus(void);
//~~~~~~~~~~~~~~~


void SensorSta(void)
{
	UpdateStatus();
	//for MidSensor1
	while(iLoop <= LoopTime)
	{
		iLoop++;
		if (MidSensor1 == 0)
		{
			_nop_();
			_nop_();
			_nop_();
			_nop_();
			iSensorConfirm++;
			_nop_();
			_nop_();
			_nop_();
			_nop_();

		}
	}
	if (iSensorConfirm >= ConfirmTime)
	{CurStaMidSensor1 =0;}
	else 
	{CurStaMidSensor1 =1;}
	iLoop = 0;
	iSensorConfirm = 0;	  //clear flag

	//for MidSensor2
	while(iLoop <= LoopTime)
	{
		iLoop++;
		if (MidSensor2 == 0)
		{
			_nop_();
			_nop_();
			_nop_();
			_nop_();
			iSensorConfirm++;
			_nop_();
			_nop_();
			_nop_();
			_nop_();
		}
	}
	if (iSensorConfirm >= ConfirmTime)
	{CurStaMidSensor2 =0;}
	else 
	{CurStaMidSensor2 =1;}
	iLoop = 0;
	iSensorConfirm = 0;

	//for HozSensor1
	while(iLoop <= LoopTime)
	{
		iLoop++;
		if (HozSensor1 == 0)
		{
			_nop_();			
			_nop_();
			_nop_();
			_nop_();
			iSensorConfirm++;
			_nop_();
			_nop_();
			_nop_();
			_nop_();

		}
	}
	if (iSensorConfirm >= ConfirmTime)
	{CurStaHozSensor1 =0;}
	else 
	{CurStaHozSensor1 =1;}
	iLoop = 0;
	iSensorConfirm = 0;


	//for HozSensor2
	while(iLoop <= LoopTime)
	{
		iLoop++;
		if (HozSensor2 == 0)
		{
			_nop_();			
			_nop_();
			_nop_();
			_nop_();
			iSensorConfirm++;
			_nop_();
			_nop_();
			_nop_();
			_nop_();

		}
	}
	if (iSensorConfirm >= ConfirmTime)
	{CurStaHozSensor2 =0;}
	else 
	{CurStaHozSensor2 =1;}
	iLoop = 0;
	iSensorConfirm = 0;



} 
//~~~~~~~~~~~Update Sensor Stutus~~~~~~~~~~
// have proble
void UpdateStatus(void) 
{
   if (LinePreStatus != LineCurStatus)
   {
 		PreStaMidSensor1 = CurStaMidSensor1;
		_nop_();
		PreStaMidSensor2 = CurStaMidSensor2;
		_nop_();
 		PreStaHozSensor1 = CurStaHozSensor1;
		_nop_();
  		PreStaHozSensor2 = CurStaHozSensor2;
		_nop_();
		iStopStatus = 0;  //clear the stop Flag
	}
	if ((PreStaHozSensor1 == CurStaHozSensor1) &&
		(PreStaHozSensor2 == CurStaHozSensor2)	)	
	{
		iStopStatus++;
	}
}