#include<reg51.h>
#include<intrins.h>  
#define u8 unsigned char
#define u16 unsigned int

//----------------------------------------------------------
//STC15寄存器声明

sfr P3M1=0xb1;
sfr P3M0=0xb2;
sfr AUXR=0x8e;
sfr T2H=0xd6;
sfr T2L=0xd7;
sfr IE2=0xaf;


//----------------------------------------------------------
//引脚定义

sbit AL=P3^0;
sbit BL=P3^1;
sbit AH=P3^2;
sbit BH=P3^3;

sbit IN=P3^4;


//----------------------------------------------------------
//MOS开关宏定义

#define AH_ON  AH=1
#define AH_OFF  AH=0
#define BH_ON  BH=1
#define BH_OFF  BH=0

#define AL_ON  AL=1
#define AL_OFF  AL=0
#define BL_ON  BL=1
#define BL_OFF  BL=0


//----------------------------------------------------------
//软件PWM

u8 PWM_count;
u8 PWM_duty_A;
u8 PWM_duty_B;

u8 PWM_value;

//----------------------------------------------------------
//控制

bit lose;
bit updata;
bit Direction;
bit Direction_last;

u16 time;
u16 timer2;



void delay_us(u8 us)		//@12.000MHz
{

	while(--us)
		_nop_(),_nop_();

}

void delay_ms(u16 ms)		//@12.000MHz
{
	u16 i;

	do{
	    i = 12000000 / 13022;
		
			_nop_(),_nop_(),_nop_();
		  while(--i)	;   
     }while(--ms);	
}



void measure()   //阻塞式脉宽测量
{
	time=0;
	TL0=0,TH0=0;
	while(!IN);
	TR0=1; 
	while(IN);
	TR0=0; 
	time=TH0;
	time<<=8;
	time+=TL0;		
}


void shock( u8 n)    //震动发声
{
	u8 i;
	u8 t=n/5;
	
	for(i=0;i<t;i++)
	{
		AH_OFF,BL_OFF;		
		delay_ms(2);
		
		BH_ON;
		
		AL_ON; delay_us(50);
		AL_OFF;delay_us(n);
		AL_ON; delay_us(50);
		AL_OFF;delay_us(n);
		AL_ON; delay_us(50);
		AL_OFF;delay_us(n);
		AL_ON; delay_us(50);
		
		
		BH_OFF,AL_OFF;		
		delay_ms(2);
		
		AH_ON;
		
		BL_ON; delay_us(50);
		BL_OFF;delay_us(n);
		BL_ON; delay_us(50);
		BL_OFF;delay_us(n);
		BL_ON; delay_us(50);
		BL_OFF;delay_us(n);
		BL_ON; delay_us(50);
		
	}
	AH_OFF,BL_OFF;
	BH_OFF,AL_OFF;
	delay_ms(5);
}


main()
{
	
	AL_OFF;BL_OFF;	//上电关闭输出
	AH_OFF;BH_OFF;
	P3M0=0x0f;      //IO推挽输出
	
	delay_ms(400);
	
	shock( 200 );	//上电音乐，表明正常工作
	shock( 150 );
	shock( 100 );
  delay_ms(600);
	
	TMOD=0x01;  //启动定时器测量
	
	measure();
	measure();
	measure();
	measure();
	measure();
	
	while(1)
	{
		measure();
		
		if(time>=1470 && time<=1530)//等待油门中位点
		{
			break;
		}
		
	}
	
	shock( 100 );	//就绪音乐，表明已检测到中位
	shock( 150 );
	shock( 200 );
	
	IE=0x80;
	IE2=0x04;
	T2L=0xF6;T2H=0xFF;  //开启软件PWM
	AUXR=0x10;
	
	measure();
	measure();
	
	while(1)
	{
	 	measure();
		measure();
		if(lose)
		{
			delay_ms(50);
			measure();
			measure();
			measure();
		}

		if(time>900&&time<2100) //只受理合理舵量范围
		{ 
			timer2=0; lose=0;
			
			if(time>=1530 )       //正转
			{
				PWM_value= (time-1520)/4; //补偿摩擦力
				Direction=0;
				updata=1;
			}
			else if(time<=1470 )  //反转
			{
				PWM_value= (1480-time)/4;
				Direction=1;
				updata=1;
			}
			else                  //停止
			{
				PWM_duty_A=0;
				PWM_duty_B=0;
				AH_OFF;
				BH_OFF;
			}
			
			if(updata)
			{
				updata=0;
				
				if(Direction != Direction_last) //运行方向改变
				{
					PWM_duty_A=0;
					PWM_duty_B=0;
					AH_OFF;
					BH_OFF;
					
					delay_ms(3);  //延时以保证上臂关闭
				}
				Direction_last=Direction;
				
				if(Direction) //AH -> BL
				{
					AH_ON;BH_OFF;
					PWM_duty_A=0;
					PWM_duty_B=PWM_value;
				}
				else          //BH -> AL
				{
					BH_ON;AH_OFF;
					PWM_duty_B=0;
					PWM_duty_A=PWM_value;
				}	
			}

		}
				 

	}
}

void ET2_isr()interrupt 12//软件PWM 1Khz
{

	PWM_count++;
	if(PWM_count==100) //100级分辨率
	{
		PWM_count=0;
		
		timer2++;
		if(timer2>500)//0.5S无信号保护
		{
			timer2=0; lose=1;
			
			PWM_duty_A=0;
			PWM_duty_B=0;
			AH_OFF;
			BH_OFF;	
			
		}
	}
	
	if(PWM_duty_A > PWM_count)AL_ON;
	else AL_OFF;
	
	if(PWM_duty_B > PWM_count)BL_ON;
	else BL_OFF;
	
}