#include "control.h"

extern  unsigned char save_flag;
extern  void Flash_Save(void);

void MotorStartCheck(void)
{		
	if(MOTORSTATE==STATEERROR)return;
		
	if(SetSpeed>0&&startflag==1)
	{		
	    MOTORSTATE=STATESTARTPID;		
			Dir=dirflag;		
      HALLcount=0;
		  HALLcount1=0;
			HALLcountTemp=0;		
	}
}
	
void MotorStartPID(void)
{		
	if(MOTORSTATE==STATEERROR)return;
 		
			PID_init();
			PIDcompute(MINSPEED,0);
			HALL_MOTOR_START();

    	TargS1=SetSpeed;
	    MOTORSTATE=STATERUNPID;	
}

void MotorRunPID(void)
{	
	
	if(MOTORSTATE==STATEERROR)return;
	if(SetSpeed==0||startflag==0) //给出停止信号
	{
	 	MOTORSTATE=STATESTOP;	
	}
	
	TargS1=SetSpeed;	
	if(TimeCountPID>=20)//计算实时速度并时行PID运算
	{
		TimeCountPID=0;
		RealS1=HALLcountTemp*500/MPolePairs;//HALLcount*100*60/6/POLEPAIRS;	
		PIDcompute(TargS1,RealS1);		 
	}
}

void MotorStop(void)
{
	if(MOTORSTATE==STATEERROR)return;
	Motor_Start_F=0;	
	MOTOR_STOP0();

  if(RealS==0) 
	{
		 MOTORSTATE=STATESTARTCHECK;	   //停下来后才切换流程 
	}			
}

void MotorError(void)
{
	MOTOR_STOP0();
	Motor_Start_F=0;
	MOTORSTATE=STATEERROROVER;
	printf("MotorError\n");
}

void MotorBreak(void)
{
	unsigned int dd;
	if(GPIO_ReadPin(BKIN_GPIO_PORT,BKIN_GPIO_PIN)==GPIO_Pin_RESET)
	{
		for(dd=0;dd<500;dd++)
		{;}
			if(GPIO_ReadPin(BKIN_GPIO_PORT,BKIN_GPIO_PIN)==GPIO_Pin_RESET)
			{
					MOTOR_STOP0();
					Motor_Start_F=0;
				while(GPIO_ReadPin(BKIN_GPIO_PORT,BKIN_GPIO_PIN)==GPIO_Pin_RESET);
			}
	}
}

void MotorErrorOver(void)
{
	unsigned char times=0;	
	
	times=0;
	
	LEDOFF();TimeCountTemp=0;
	while(TimeCountTemp<200);
	while(1) //发生故障，则故障指示灯闪烁。需重启
	{
		SampleSpeed();
		if(SetSpeed == 0) //故障时，不加速后恢复
		{
			ErrorCode = 0;
			WaitStart();
			GPIO_WritePin(VOUT_EN_GPIO_PORT,VOUT_EN_GPIO_PIN,GPIO_Pin_SET);
			break;
		}

	}
}

void WaitStart(void)
{
	if(SetSpeed==0) MOTORSTATE=STATESTARTCHECK;	
}

void EnDirCheck(void)
{
	

  if((GPIO_ReadPin(EN_GPIO_PORT,EN_GPIO_PIN)==GPIO_Pin_SET)\
		&&(GPIO_ReadPin(DR_GPIO_PORT,DR_GPIO_PIN)==GPIO_Pin_SET) )
	{
		LevelSet = 2;
	}
	else if((GPIO_ReadPin(EN_GPIO_PORT,EN_GPIO_PIN)==GPIO_Pin_RESET)\
		&&(GPIO_ReadPin(DR_GPIO_PORT,DR_GPIO_PIN)==GPIO_Pin_SET) )
	{
	   LevelSet = 3;
	}
	if((GPIO_ReadPin(EN_GPIO_PORT,EN_GPIO_PIN)==GPIO_Pin_SET)\
		&&(GPIO_ReadPin(DR_GPIO_PORT,DR_GPIO_PIN)==GPIO_Pin_RESET) )
	{
		LevelSet = 1;
	}
	printf("Level=%d\n",LevelSet);			 	 
}