#include "ys32md211.h"
#include "motor.h"
#include "M_speed.h"
#include "pwm.h"
#include "key.h"
uint16_t  Speed_min_inc;
uint8_t  Speed_rms_flag;
void speed_int(void)
{
   pi_init();
   m_par_set.speed_loop_state=MC_LOOP;//开环和闭环设置
}

/***************************************************
函数名称：mc_speed_calc(void)
功能：    电机运行速度计算
备注：+
****************************************************/
unsigned long int speed_time_sum =0,speed_time=0,speed_add_times=0; 
unsigned int speed_temp = 0; 
void mc_speed_calc(void)
{
	//if(mc.state == MC_START || mc.state == MC_RUN)
	if(m_par_set.commutate60_flag)//&&(m_par_set.Motor_state == M_RUN))
	{
		 m_par_set.commutate60_flag=0;
		 if(m_par_set.Motor_state!=M_STOP)
		    speed_time_sum += (m_par_set.com_common_sum_60*PWM_TIM);// m_par_set.com_common_time_60;//
		 else
			  speed_time_sum += (m_par_set.com_common_time_60*PWM_TIM);
		 speed_add_times++;
		 if(speed_add_times >= 32)
		 {
			  speed_time = speed_time_sum >>5; 
			  speed_time_sum = 0; 
			  speed_add_times = 0; 
			  if(speed_time!=0)
			  {
				  #if(BLDC_MOTOR == SENSOR_HALL)	
						 speed_temp=SYS_CLK/48/speed_time /6.0 * 60 / POLE;//10000000/speed_time / POLE;// DeSpeed_time/POLE/speed_time;//SYS_CLK/48/speed_time * 60 / POLE;
						 hall.speed = speed_temp; 		
				  #elif(BLDC_MOTOR == SENSOR_NONE)	
						 speed_temp= SYS_CLK/48/speed_time /6.0 * 60 / POLE;// 10000000/speed_time / POLE; //DeSpeed_time/POLE/speed_time;//SYS_CLK/48/speed_time /6.0 * 60 / POLE;
				  #endif	
				    m_par_set.actual_speed = (m_par_set.actual_speed * 2 + speed_temp*8)/10;		
			  }
			  else 
			   {
				    m_par_set.actual_speed = 0; 
			   }
		 }
		 
		 if(m_par_set.Motor_state==M_STOP)
		 {
		     m_par_set.H_duty =m_par_set.actual_speed/m_par_set.speed_unit*4/5;
		 }
		    
		 if(m_par_set.Motor_state==M_RUN)
		 {
		     m_par_set.speed_unit=m_par_set.actual_speed/m_par_set.H_duty;
		 }
		 if(m_par_set.speed_unit<100)
			  m_par_set.speed_unit=100;
  }
}

/***************************************************
函数名称： mc_ramp(unsigned int set_value, unsigned int actual_value)
功能：    电机运行过程中加速减速爬坡函数
备注：  set_value:电机设置值  actual_value:电机当前值， 爬坡相应参数在user_config文件中设置
****************************************************/
//unsigned int set_value = 0;
//unsigned int actual_value = 0; 
//void mc_ramp(unsigned int *set_value1, unsigned int *actual_value1)
//{
//	unsigned int *set_value = set_value1; 
//	unsigned int *actual_value = actual_value1; 
//	if(m_par_set.Motor_state == 4)//if(m_par_set.state == MC_RUN)
//	{
//		m_par_set.ramp_cnt++; 
//		if(m_par_set.ramp_cnt >= RAMP_PERIOD)
//		{
//			m_par_set.ramp_cnt = 0; 
//			if(*set_value > *actual_value)
//			{
//				if(*set_value - *actual_value > RAMP_ADD_SPEED)
//				{
//					*actual_value += RAMP_ADD_SPEED;
//				}
//				else
//				{
//					*actual_value = *set_value;
//				}
//			}
//			else if(*set_value < *actual_value)
//			{
//				if(*actual_value - *set_value > RAMP_SUB_SPEED)
//				{
//					*actual_value -= RAMP_SUB_SPEED;
//				}
//				else
//				{
//					*actual_value = *set_value;
//				}				
//			}
//			else
//			{
//				*actual_value = *set_value;
//			}
////			return actual_value; 
//		}
//	}
//}

/***************************************************
函数名称：pi_init(void)
功能：    pi参数初始化
备注：
****************************************************/
void pi_init(void)
{
	pi.kp = START_KP;
	pi.ki = START_KI;
	pi.out = 0;
	pi.ui = 0; 
	pi.up = 0; 
	pi.pi_cnt = 0; 
}
/***************************************************
函数名称：pi_control(unsigned int set_speed, unsigned int actual_speed)
功能：    pi计算,输出计算值
备注：    相应参数在user_config中配置
****************************************************/
unsigned int pi_control(unsigned int set_speed, unsigned int actual_speed)
{
  int err; 
	err = set_speed - actual_speed; 
	pi.up = pi.kp * err; 
	pi.ui +=pi.ki*err; 
	if(pi.ui >= LIMIT_MAX)
	{
		pi.ui = LIMIT_MAX;
	}
	else if(pi.ui < LIMIT_MIN)
	{
		pi.ui = LIMIT_MIN; 
	}
	pi.out = pi.up+pi.ui; 
	if(pi.out >= LIMIT_MAX)
	{
		pi.out = LIMIT_MAX; 
	}
	else if(pi.out <= LIMIT_MIN)
	{
		pi.out = LIMIT_MIN; 
	}
	return pi.out; 
}
//************************************************
void  speed_add_process(void)
{
	  m_par_set.ramp_cnt++;
    if(m_par_set.ramp_cnt<RAMP_PERIOD)
			return;
		m_par_set.ramp_cnt=0;	 
	  if(m_par_set.actual_duty>m_par_set.H_duty)
				m_par_set.H_duty++;     	
    if(m_par_set.actual_duty<m_par_set.H_duty)// else
		{
				if(m_par_set.H_duty>LIMIT_MIN)
				{
						 m_par_set.H_duty--;

				}   	
        if(m_par_set.H_duty>LIMIT_MAX)	
             m_par_set.H_duty--;							 
		}
    PWM_SET_DUTY(m_par_set.H_duty);
}
/***************************************************
oˉêy??3?￡oloop_control(void)
1|?ü￡o    ?·?·????
±?×￠￡o    
****************************************************/

unsigned pro_time  = 0;
void Speed_loop_control(void)
{
	
	//if(m_par_set.Motor_state ==4)
	 
	if(m_par_set.speed_loop_state == 0)    //?a?·
		{
      //#if(MC_LOOP == OPEN_LOOP)			
			 // mc_ramp(&m_par_set.set_duty,&m_par_set.actual_duty);
				//PWM_SET_DUTY(m_par_set.actual_duty);	
			   if(Speed_rms_flag==0)
		   return;
         speed_add_process();//			

		}
		else                      //±??·
		{
	
		      #if(LIMIT_CUR_ENABLE ==1)   
            limit_current(&mc.set_speed);
			    #endif
			    m_par_set.set_value = m_par_set.set_speed; 
			    m_par_set.actual_value = m_par_set.actual_speed;
		    	//mc_ramp(&m_par_set.set_value,&m_par_set.actual_value);
			    m_par_set.actual_duty = pi_control(m_par_set.set_value,  m_par_set.actual_value);
			    if(Speed_rms_flag==0)
		   return;
			    speed_add_process();

		}
	

}
//*******************************************************//

void Speed_rms_confir(void)
{
	 if(m_par_set.Motor_state <M_RUN)
	 {
      Speed_min_inc=0;
		  Speed_rms_flag=0;
		  return;
	 }
	 Speed_min_inc++; 
	 if((m_par_set.Vali_BEM_data>M_BMF_TIME)&&(m_par_set.actual_speed>LOOP_SPEED_MIN))//&&(m_par_set.com_common_time_60<START_SPEED_LOOP_VALUE))
	    Speed_min_inc++;
	 else
	 {
		  Speed_min_inc=0;
		  if(!Speed_rms_flag)
		  Speed_rms_flag=0;
	 }
	 if(Speed_min_inc>=300)
		  Speed_rms_flag=1;

}
////************************************************************************//
////   速度小于设置最小速度关机
////*************************************************************************//
//uint8_t   t_20ms_inc;
//uint16_t  G_speed_check[3];
//uint8_t   G_speed_index;
//void Locked_motor_process(void)
//{
//	  uint16_t  speed_check_temp=0;
//	  if(m_par_set.Motor_state<M_RUN)
//		  m_par_set.m_lock_time=0;
//    m_par_set.m_lock_time++;  
//    if(m_par_set.m_lock_time>=LOCK_START_MIN_TIME)	
//		{
//			  speed_check_temp=START_SPEED_MIN_VALUE/2;
//			  if(m_par_set.com_common_time_60>speed_check_temp)//小于设置最小速度关机
//				{
//				    clr_key_state();
//				}
//				if(m_par_set.m_lock_time>=LOCK_START_MAX_TIME)
//				{
//					 m_par_set.m_lock_time=LOCK_START_MAX_TIME;
//		       t_20ms_inc++;
//				   if(t_20ms_inc>=20)
//				   {
//				      t_20ms_inc=0;
//						  if(G_speed_index<3)
//							{
//							   G_speed_check[G_speed_index]=m_par_set.com_common_time_60;
//								 G_speed_index++;
//							}
//							else
//							{
//							   G_speed_check[0]=G_speed_check[1];  
//							   G_speed_check[1]=G_speed_check[2];
//								 G_speed_check[2]=m_par_set.com_common_time_60;
//							}
//						  
//				   }
//					 if(G_speed_index>2)
//					 {

//					 }
//					 
//				}
//				else
//				{
//					G_speed_index=0;
//					t_20ms_inc=0;
//				}
//		}
//		else
//		{
//		    t_20ms_inc=0;
//		}
// // #endif
//}
////*****************************************************//
////换相异常关机
////****************************************************//
//void M_stall_process(void)
//{
//    if(m_par_set.m_lock_time<=LOCK_START_MIN_TIME)
//		{
//			 m_par_set.Abnor_time=0;
//			 m_par_set.stall=0;
//		   return;
//		}
//    m_par_set.Abnor_time++;
//		if(m_par_set.Abnor_time>=1000)
//		{
//			 m_par_set.Abnor_time=0;
//		   if(m_par_set.stall>=3)
//			 {
//				  clr_key_state();
//				  
//			 }
//			 m_par_set.stall=0;
//		}
//}

//***************************************************//
void Speed_process(void)
{
   Speed_rms_confir();
   Speed_loop_control();

}
