#include <REGX52.H>
#include "Delay.h"
#include "Motor.h"
#include "HC-SR04.h"
#include "Sg90.h"

//避障模式

unsigned char MiddleFlag;		//超声波方向朝前标志位
double MiddleDis,LeftDis,RightDis;


/**
  * @brief 舵机初始化函数，程序一开始让超声波模块往前
  * @param 无
  * @retval无
  */
void HC_Init()
{
	SgMiddle();		//一开始为90度，往前
	Delay1ms(300);
	MiddleFlag = 1;	//标志位置1
}


/**
  * @brief 开始左右摇头避障
  * @param 无
  * @retval无
  */
void Start_Avoid()
{
	if(MiddleFlag != 1)				//避免舵机抽搐
	{
		SgMiddle();
		Delay1ms(180);
		MiddleFlag = 1;
	}
	MiddleDis = ultrasonic();		//检测前方距离
	if(MiddleDis > 30)				//前方没有障碍
	{
		GoForward();				//前进
	}
	else if(MiddleDis < 10)			//如果距离过小
	{
		GoBack();					//后退
	}
	else							//前方有障碍
	{
		//停止
		Stop();
		SgLeft();					//180度，左扭头
		Delay1ms(180);
		LeftDis = ultrasonic();		//测左边距离
		
		SgMiddle();					//回到中间
		Delay1ms(180);
		
		SgRight();					//0度，右扭头
		Delay1ms(180);
		RightDis = ultrasonic();	//测右边距离
		
		//若左边距离大于右边距离，说明左边宽敞，向左转
		if(LeftDis > RightDis)	
		{
			GoLeft_Avoid();
			Delay1ms(170);
			Stop();
		}
		//若右边距离大于左边距离，说明右边宽敞，向右转
		if(RightDis > LeftDis)
		{
			GoRight_Avoid();
			Delay1ms(170);
			Stop();
		}
		MiddleFlag = 0;
	}
}