#include "FocStartup.h"
#include "FocConfig.h"
#include "string.h"
#include "arm_math.h"


/*******************************************************************************
 * @brief  初始化参数
 * @param   
 * @return  
 ******************************************************************************/
void IfStartupInit(ifStartup_t *pHandle)
{
	memset(pHandle,0,sizeof(ifStartup_t));
}
/*******************************************************************************
 * @brief   if启动
 * @param   pHandle if实例 obTheta观测器角度
 * @return  
 ******************************************************************************/
void IfStartupStep(ifStartup_t *pHandle,float obTheta,float sin_cos[])
{
	float theta;
	switch(pHandle->state)
	{
		case 0://预定位阶段
			if(pHandle->iq < ALIGIN_CURRENT)
			{
				 pHandle->iq += (ALIGIN_CURRENT * FOC_PWM_PERIOD );
				 theta = ALGIN_THETA; 
			}
			else
			{
				pHandle->state = 1;
				theta = ALGIN_THETA + 0.10f * 6.28f;
			}
			break;
		case 1://加速阶段
			pHandle->iq = IF_STARTUP_IQ;
			if(pHandle->omega <= IF_STARTUP_OMEGA)
			{
					pHandle->omega += IF_STARTUP_OMEGA_ACC * FOC_PWM_PERIOD;
			}
			else
			{
				pHandle->state = 2;
			}
			pHandle->theta += pHandle->omega * FOC_PWM_PERIOD;
			if(M_2PI <= pHandle->theta)
			{
				pHandle->theta -= M_2PI;
			}
			if(0 > pHandle->theta)
			{
				pHandle->theta += M_2PI;
			}
			pHandle->time += FOC_PWM_PERIOD;
			theta = pHandle->theta;
			break;
		case 2://匹配阶段
			pHandle->theta += pHandle->omega * FOC_PWM_PERIOD;
			if(M_2PI <= pHandle->theta)
			{
				pHandle->theta -= M_2PI;
			}
			if(0 > pHandle->theta)
			{
				pHandle->theta += M_2PI;
			}
			pHandle->thetaDiff = obTheta - pHandle->theta;
			if(pHandle->thetaDiff > 0)
			{
				pHandle->thetaDiff = 6.28f - pHandle->thetaDiff;
			}
			//获取绝对值
			float error = pHandle->thetaDiff ;
			if(error < 0)
			{
				error = -error;
			}
			if(error > 6.28f)
			{
				error = 6.28f;
			}
			pHandle->iq -= error * IF_STARTUP_IQ_KP;
			if(pHandle->iq < IF_STARTUP_IQ_MIN)
			{
				pHandle->iq = IF_STARTUP_IQ_MIN;
			}
			if(error <= IF_STARTUP_THETA_DIFF)
			{
				pHandle->diffCnt++;
			}
			else
			{
				pHandle->diffCnt = 0;
			}
			pHandle->time += FOC_PWM_PERIOD;
			if(pHandle->diffCnt > 10 || pHandle->time >= IF_STARTUP_MAX_TIME)
			{
					pHandle->state = 3;
			}
			theta = pHandle->theta;
			break;
		case 3://进入观测器
			theta = obTheta;
			break;
	}
	sin_cos[0] = arm_sin_f32(theta);
	sin_cos[1] = arm_cos_f32(theta);
}