#include "DC2AC.H"
#include "ctrl.h"
#include "task.h" 
 

#define PULSE_PRIOD 800UL //20KHz开关频率
extern unsigned char Warning_flag;
double voltage[7];  //电压

#define VOLTAGE24   	voltage[0]
#define BAT_POWER			voltage[1] 
#define SENSOR_HEAT  	voltage[2]   
#define SENSOR_SMOKE 	voltage[3]
#define MAINPOWER			voltage[4]
#define COILVOL     	voltage[5]
#define VOLPOT				voltage[6]

  
void InitDC2AC(void* p)
{ 
	TIM1_DeInit();       //16MHz
	TIM1_TimeBaseInit(0,TIM1_COUNTERMODE_UP,PULSE_PRIOD, 0);
	TIM1_Cmd(ENABLE);
	StartPulse(0);
 	// TIM1 Main Output Enable 使能TIM1外设的主输出
 	TIM1_CtrlPWMOutputs(ENABLE);   
 	// TIM1 counter enable开定时器 
	TIM1_Cmd(ENABLE);        
}

void LockAllMotor(void)
{
	(GPIOC->ODR&=(~0x08));
}

void UNLockAllMotor(void)
{
	(GPIOC->ODR|=0x08);
}

void StartPulse(void* p)
{
	IED.MotorMoveDelayTimerID=-1;
	TIM1_OC1Init(TIM1_OCMODE_PWM1, 
                  TIM1_OUTPUTSTATE_ENABLE, 
                  TIM1_OUTPUTNSTATE_DISABLE, 
                  PULSE_PRIOD/2, 
									TIM1_OCPOLARITY_LOW , 
                  TIM1_OCNPOLARITY_LOW , 
                  TIM1_OCIDLESTATE_RESET, 
                  TIM1_OCNIDLESTATE_RESET);
	TIM1_OC1PreloadConfig(ENABLE);
	TIM1_OC2Init(TIM1_OCMODE_PWM1, 
                  TIM1_OUTPUTSTATE_ENABLE, 
                  TIM1_OUTPUTNSTATE_DISABLE, 
                  PULSE_PRIOD/2, 
									TIM1_OCPOLARITY_HIGH , 
                  TIM1_OCNPOLARITY_LOW , 
                  TIM1_OCIDLESTATE_RESET, 
                  TIM1_OCNIDLESTATE_RESET); 
	TIM1_OC2PreloadConfig(ENABLE);  
}

void InitInvert(void *p)
{
		//自举升压
	TIM1_OC3Init(TIM1_OCMODE_PWM1,               
                  TIM1_OUTPUTSTATE_ENABLE, 
                  TIM1_OUTPUTNSTATE_DISABLE, 
                  PULSE_PRIOD/2, 
									TIM1_OCPOLARITY_HIGH , 
                  TIM1_OCNPOLARITY_LOW , 
                  TIM1_OCIDLESTATE_RESET, 
                  TIM1_OCNIDLESTATE_RESET); 
	TIM1_OC3PreloadConfig(ENABLE); 
}

void StopInvert() 
{ 
	TIM1_ForcedOC3Config(TIM1_FORCEDACTION_INACTIVE );
}


void DeInitDC2AC(void *p)
{
	IED.cDCCovert = 1;
	TIM1_ForcedOC1Config(TIM1_FORCEDACTION_INACTIVE );
	TIM1_ForcedOC2Config(TIM1_FORCEDACTION_ACTIVE );
	//为 啥通道3InActive为高电平呢?为啥和通道2是反的呢?
																//和输出极性有关 ?
}
 
void StopDC2ACConvert(void* p)
{
	IED.cMoving = 0;  
	IED.cDCCovert = 0;
	#ifdef _DEBUG
	printf("停止直流逆变\n  " );
	#endif  
	StopTimer(&IED.MotorMoveDelayTimerID );
	DeInitDC2AC(0); 									 	   		   
}   

int CCR2_Val = 0; 
void SetWarningFreq(unsigned  int freq)
{  
	TIM3_OCMode_TypeDef mode;
	TIM3_DeInit();       //4MHz    //溢出频率为1KHz     //实际250Hz
	TIM3_TimeBaseInit(0,0xFFFF);
	TIM3_Cmd(ENABLE);  
	
	if(freq==0) 
	 	mode			= TIM3_OCMODE_TIMING;     
	else 
	{  
		CCR2_Val = 8000000UL/freq;
		if((TIM3->CCMR2&TIM3_OCMODE_TOGGLE)==TIM3_OCMODE_TOGGLE) 
			return ; 
		mode		= TIM3_OCMODE_TOGGLE;   
		TIM3_ITConfig(TIM3_IT_CC2, ENABLE);
	} 
	TIM3_OC2Init(mode,
                  TIM3_OUTPUTSTATE_ENABLE,
									CCR2_Val, 
                  TIM3_OCPOLARITY_LOW); 
	
}
 

#ifdef _COSMIC_
@far @interrupt void TIM3_CAP_COM_IRQHandler(void)
#else // _RAISONANCE_  
void TIM3_CAP_COM_IRQHandler(void) interrupt 16
#endif // _COSMIC_  
{ 
	unsigned int  newValue = (TIM3->CCR2H<<8)+TIM3->CCR2L+CCR2_Val; 
	TIM3->SR1 &=  ~TIM3_IT_CC2;  
	TIM3->CCR2H = newValue>>8;
	TIM3->CCR2L = newValue&0xFF;
	GPIOD->ODR^=0x80;
}  

void EnableBrakPower (UCHAR bEnable)
{
	if(IED.BrakeDelayTimerID!=-1)
		StopTimer(&IED.BrakeDelayTimerID);
	if(bEnable)
	{
		BRAKECHECK = 1;	   //如果要开刹车,立即打开 
//		BREAKSELE = (IED.Err&(ErrPhase|MAIN_POWER_ERROR))? DC_BREAK:AC_BREAK	; 
		SuspendTask(TASK_CHECK_COIL);
		SuspendTask(TASK_CHECK_MOTOR); 
		#ifdef _DEBUG
		printf("停止刹车检测\n ");
		#endif	 		
	}
	else			   //如需检测刹车,延时放开,防止反电势干扰
	{ 
		IED.BrakeDelayTimerID =  StartTimer(
					TIMER_MODE_ONCEROUTINE,1000UL,StartCheckBrake,0);
		nop();
	}
	
} 

 void StartCheckBrake(void *p)
{
	IED.BrakeDelayTimerID = -1;
	BRAKECHECK = 0;
	ActiveTask(TASK_CHECK_COIL,50L);
	 ActiveTask(TASK_CHECK_MOTOR,1000L);
	#ifdef _DEBUG
		printf("启动刹车检测\n ");
	#endif	 		
	//启动检测任务
	//BRAKECHECK
}


void EnableBattary(void)
{
	if(BAT_CTRL_RELAY!=1)
	{
		#ifdef _DEBUG
		printf("连接电池\n ");
		#endif	 		
		BAT_CTRL_RELAY = 1;
	}
}

void DisableBattary(void)
{  
	if(BAT_CTRL_RELAY!=0)
	{
		#ifdef _DEBUG
		printf("断开电池\n ");
		#endif	
		BAT_CTRL_RELAY = 0;
	}
}

         
void BotCheck(void)
{
	GetCurretPos();
	if(pos_bot_bit==0)
	{
		StopDC2ACConvert(0);
		SuspendTask(TASK_BREAK);//DC_DN_PR
		#ifdef _DEBUG
		printf("卷帘下滑到底,停止逆变");
		#endif               
		IED.cPos = POS_BOT ; 
	}
	else
	{		 
		CheckBrakePower();//检查逆变是否正常
		if(Warning_flag==0)
			Beep(1,200); 
	}
} 

bool DC2AC_CTRL = FALSE;
void StartDC2ACConvert(void)
{
	IED.cMoving = 1;
	if(DC2AC_CTRL==FALSE)
	{
		DC2AC_CTRL	= TRUE; 
		
		UNLockAllMotor();
		//SelectBrakPower(DC_BREAK,TRUE);
		//InitDC2AC();
		#ifdef _DEBUG
		printf("启动直流逆变\n " );
		#endif      
	}
	StopTimer(&IED.MotorMoveDelayTimerID);
	IED.MotorMoveDelayTimerID =  StartTimer(TIMER_MODE_ONCEROUTINE,10,StartPulse,0);	  //延时0.1S逆变
 
}
 
 

#define MAINPOWER_VOL  3000UL	  //发光
#define BATLOW_VOL     9000UL   //欠压
#define BAT_TH_VOL		 12000UL 
#define BATSHORT_VOL   1000UL   //短路
#define BATFULL_VOL    12400UL	//冲满
#define BATOVER_VOL    12600UL  //过压
void Task_Power_Check()		//电压错误信号检测
{ 			
	if(MAINPOWER>MAINPOWER_VOL)	   // 主电电压正常显示
	{	
      IED.Main_Power_OK_Cnt++  ;   //连续电压好才判断正常
      if(IED.Main_Power_OK_Cnt>1)
			{				 
				IED.Err &= ~MAIN_POWER_ERROR; 
				ActiveTask(TASK_PHASE,PHASE_PRIOD); 
				IED.Main_Power_OK_Cnt = 1;
			}                              
	}
	else						  //主电电压不正常显示
	{	
		DeChargeBat(FALSE);	 //停止电池放电
		IED.Main_Power_OK_Cnt = 0;  
		if(MOTOR__RUN)   
			ErrStopMotor();       
		IED.Err |= MAIN_POWER_ERROR; 
		IED.Err |= ErrPhase;
		SuspendTask(TASK_PHASE);						//主电错误停止相位检测,防止检测错误
	} 
}

void ChargeBat(bool bEnable)
{						    

	if(bEnable)				//充电时不得放电
		DeChargeBat(FALSE);	//不充电时不一定放电
	CHARGE_BAT = bEnable;
}

void DeChargeBat(bool bEnable)
{
	FLUSH_BAT = bEnable;
}


void BatCheck(void)
{
	#ifdef _DEBUG
//	if((IED.Err!=0x05)&&(IED.Err!=0x00))
//		printf("IED.Err=0x%04X\n",IED.Err);
	#endif
	if(BAT_POWER <BATLOW_VOL)  	   						//备电电压低
	{ 													//9.0--13.9V
		if(MAINPOWER>MAINPOWER_VOL)
		{
			IED.Bat_Low_Cnt =0 ;		 
		  IED.Err &= ~BAT_LOW_ERR;
		}
		else
		{
			IED.Bat_Low_Cnt ++ ;
			if(IED.Bat_Low_Cnt>=10)	
			{
				IED.Err |= BAT_LOW_ERR;
			}
			if(IED.Bat_Low_Cnt>=80)		//检测电压低80次后关机 8S
			{		 
				DisableBattary();	  		//电压低无主电关机
				StopDC2ACConvert(0);
			} 
		} 			  
	}
	///////////////////////////////////////////////
	if(BAT_POWER>BATLOW_VOL)//主电失电,可以检测电压低是否恢复
	{
		if((IED.Err&MAIN_POWER_ERROR)==MAIN_POWER_ERROR)	   
		//主电电压低
		{ 						 
			IED.Bat_Low_Cnt = 0; 
			IED.Err &= ~BAT_LOW_ERR; 
		}	 										
	}
  
}


enum  CHAREGE_STATE{CHARGE_INIT=0,PRE_ENCHARGE,ENCHARGE1,
CHARGE_WAIT1,DECHARGE,CHARGE_WAIT2,CHECKVOL,CHARGEEND};
//
int charegeState = 0;
int chargeRunCnt = 0;
void TaskChargeBat(void)
{
	chargeRunCnt++;
	GetVoltage();
	BatCheck();
	switch (charegeState)
	{
		case CHARGE_INIT: 
			//主电OK
			if((IED.Err&(MAIN_POWER_ERROR))==0)	   //主 电正常充电
			{  
				if(BAT_POWER>BATSHORT_VOL)
				{				
					EnableBattary();
					chargeRunCnt = 0; 
					ChargeBat(TRUE);
					charegeState = PRE_ENCHARGE;   
					#ifdef _DEBUG
					//printf("开始充电\n");
					#endif
				}
				else
				{
					IED.Err |=BAT_SHORT	; 
				}
			} 
			else
			{ 
				charegeState = CHARGE_WAIT2;
				ChargeBat(FALSE); 
				DeChargeBat(FALSE);
			}
			break;
		case PRE_ENCHARGE: 
			if(chargeRunCnt>0)
			{	 
				//////////////////////////////////////	 			
				if(BAT_POWER <BATSHORT_VOL)	
				{   
					ChargeBat(FALSE); 
					charegeState = CHARGE_WAIT2;
					IED.Err |=BAT_SHORT	; 
				} 
				else 
				{ 
					IED.Err &= ~BAT_SHORT; 
					//////////////////////////////////////// 
					chargeRunCnt = 0;
					charegeState = ENCHARGE1;  
				}
			}
		case ENCHARGE1:				 //2S充电
			if(chargeRunCnt>20)	
			{ 
				#ifdef _DEBUG
				//printf("电池充电%d\n",chargeRunCnt);
				#endif
				ChargeBat(FALSE);
				charegeState = CHARGE_WAIT1;
				chargeRunCnt = 0; 
			}	
			break;
		case  CHARGE_WAIT1:
			if(chargeRunCnt>0)		 //100ms等待
			{ 
				#ifdef _DEBUG
				//printf("开始放电\n");
				#endif
				
				if(BAT_POWER>BATLOW_VOL)  
					DeChargeBat(TRUE);
				charegeState = DECHARGE;
				chargeRunCnt = 0;
			}
			break;
		case DECHARGE: 
			if(chargeRunCnt>=2)		//0.2s放电
			{
				DeChargeBat(FALSE);
				charegeState = CHARGE_WAIT2;
				chargeRunCnt = 0; 
				#ifdef _DEBUG
				//printf("电池放电%d\n",chargeRunCnt);
				#endif
			}
			break;
		case  CHARGE_WAIT2:
			if(chargeRunCnt>10)		//1s稳定
			{							 
				charegeState = CHECKVOL;
				chargeRunCnt = 0;  
				if(BAT_POWER >BATOVER_VOL)  	   				
				//电池损坏
				{		  
					IED.Err |= BAT_OVER_ERROR;
				}
				else 
				{		 
					IED.Err &= ~BAT_OVER_ERROR; 
				}
				
				if(BAT_POWER >BATLOW_VOL)  	   						//备电电压低
				{ 						 
					IED.Bat_Low_Cnt = 0; 
					IED.Err &= ~BAT_LOW_ERR; 
				}	 
			}
			break;
		case CHECKVOL:  
			_nop_();
		 	if(BAT_POWER>BATFULL_VOL)
			{ 
				#ifdef _DEBUG
				printf("电池充满\n");
				#endif
				charegeState = CHARGEEND;
				ChargeBat(FALSE);
			}
			else
			{
				charegeState = CHARGE_INIT;
			}
			
			break;
		case CHARGEEND: 
			if(BAT_POWER<BAT_TH_VOL)//(BATFULL_VOL+BATLOW_VOL)/2)	  
			{
				#ifdef _DEBUG
				printf("开始充电\n");
				#endif
				charegeState = CHARGE_INIT;
				//_asm("nop");
			}
			
			break;
		default:
			break;
	} 
}

void GetVoltage(void)
{
	int i =0;
	Disable();
	for(i=0;i<M;i++)
	{
		voltage[i] = (AD_Value[i]*VREF )/0x03FF;  
	}
	Enable();
	VOLTAGE24 	=  11.0f*voltage[0]; 	 
	voltage[1]  =  6.0f*voltage[1];		//电池电压
	voltage[2]  =  1.0f*voltage[2];
	voltage[3]  =  1.0f*voltage[3];
	voltage[4]  =  8.7f*voltage[4];		//电源电压
	voltage[5]  =  5.7f*voltage[5];		// 
	
	#ifdef _DEBUG   
	//printf("VOLPOT=%d\n",(int)VOLPOT);  
	#endif           
} 

//#define VOLTAGESHORTVOL  4200.0f //4.2
#define VOLTAGESHORTVOL  650.0f //4.2
#define VOLTAGEBreakVOL  3870.0f 		 //电池电压9.0V为3870//6.5
int MotorLineCnt = 0;//电机检测
int PreMotorLine =0;
void Task_Check_Motor (void)
{       
    #ifndef _SINGLE
    if(MotorLineCnt>40)
    {  
        IED.Err|=ERR_MOTOR_LINE;
    } 
    else 
    {
        IED.Err&=(~ERR_MOTOR_LINE);
    }
    MotorLineCnt = 0;
    #endif
}
void TaskFREQMotor(void)
{
    if(DOORCHECK!= PreMotorLine)
    {
        MotorLineCnt++;
        PreMotorLine = DOORCHECK;
    }    
}
void TaskCheckBrakeCoil(void)
{
	if(!BRAKECHECK)
	{ 
		if(COILVOL>VOLTAGEBreakVOL)	 //线圈断开
		{
			IED.Err|=COIL_BREAK_ERR ;
		}
		else
		{
			IED.Err&=(~COIL_BREAK_ERR); 	
		}
		////////////////////////////////
		if(COILVOL<VOLTAGESHORTVOL )	//线圈短路
		{
			IED.Err|=COIL_SHORT_ERR;
		}		
		else
		{
			IED.Err&=(~COIL_SHORT_ERR); 
		} 
		
  	
	}
//	BREAKSELE = (IED.Err&(ErrPhase|MAIN_POWER_ERROR))? DC_BREAK:AC_BREAK	;
}

void CheckRelay(void)
{
	//PC3  lin
	//PA2  relay fail
		static char FailCnt2 = 0;
		if(GetLinState()) //没有被锁定
		{
			if(MOTOR__RUN!=0)
			{
//				if(GetRelayState()==0)  ////为高正常,有输出
//				{  
//					FailCnt2++;
//					if((FailCnt2>50)||
//					((IED.cDCCovert==0)&&(FailCnt2>20)))//逆变的时候
//					{
	//					IED.Err|=ERR_RELAY_FAIL;
	//					LockAllMotor();
	//				}
	//			} 
	//			else
		//		{
		//			FailCnt2>0?--FailCnt2:0;
		//		}
			}
		} 
}


void CheckBrakePower(void)
{
	//PC3  lin
	//PA2  relay fail
		static char FailCnt1 = 0;
		char LinState = GetLinState();
		if(LinState!=0) //没有被锁定		
		{
			if(GetBrakePowerState()!=0)
			// 为低正常,有输出||//逆变的时候
			{  
				FailCnt1++;
				if((FailCnt1>50)||
					((IED.cDCCovert==0)&&(FailCnt1>20)))//逆变的时候
				{
					IED.Err|=ERR_BRAKE_POWER_FAIL;
					LockAllMotor();
				}
			}
			else
			{
				FailCnt1>0?--FailCnt1:0;
			}
		}  
	
}
void TaskCheckSensor(void)
{
	if((SENSOR_HEAT>=1500.0f)||(SENSOR_SMOKE>=1500.0f))
	{
		if(IED.b_warning==0)
		{
			IED.b_warning=1;
		}
		 
	}
  else
  {
	IED.b_warning=0;
  }
	
}