#include "pid.h"
#include "stdlib.h"
/***************************
* 函数名称：void INC_Pid_Calc(int32_t ref_temp,int32_t fdb_temp,SPEED_PID_DEF* speed_pid_temp)
* 函数功能：增量式PID
* 入口参数：无
* 返 回 值：无
****************************/
void INC_Pid_Calc(int32_t ref_temp,int32_t fdb_temp,PID_DEF* speed_pid_temp)
{
int32_t tmpErr=0;
int32_t tmpUp=0;
int32_t tmpUi=0;
speed_pid_temp->err_old = speed_pid_temp->err;
speed_pid_temp->err = ref_temp-fdb_temp;
if(speed_pid_temp->err>=speed_pid_temp->err_max&&speed_pid_temp->err_max!=0)
{
speed_pid_temp->err=speed_pid_temp->err_max;
}
if(speed_pid_temp->err<=speed_pid_temp->err_min&&speed_pid_temp->err_min!=0)
{
speed_pid_temp->err=speed_pid_temp->err_min;
}
tmpErr = speed_pid_temp->err-speed_pid_temp->err_old;
tmpUp = tmpErr*speed_pid_temp->P_Gain_q12;
tmpUp>>=12;
tmpUi = speed_pid_temp->err*speed_pid_temp->I_Gain_q12;
tmpUi>>=12;
speed_pid_temp->result_out+=(tmpUp+tmpUi);
if(speed_pid_temp->result_out > speed_pid_temp->Max_Output) speed_pid_temp->result_out = speed_pid_temp->Max_Output;
if(speed_pid_temp->result_out < speed_pid_temp->Min_Output) speed_pid_temp->result_out = speed_pid_temp->Min_Output;
}
/***************************
* 函数名称：void PosPidCalc(int32_t ref_temp,int32_t fdb_temp,SPEED_PID_DEF* speed_pid_temp)
* 函数功能：位置式PID
* 入口参数：无
* 返 回 值：无
****************************/
void PosPidCalc(int32_t ref_temp,int32_t fdb_temp,PID_DEF* speed_pid_temp)
{
int32_t tmpErr=0;
int32_t tmpUp=0;
int32_t tmpUd=0;
speed_pid_temp->err_old = speed_pid_temp->err;
speed_pid_temp->err = ref_temp-fdb_temp;
if(speed_pid_temp->err>=speed_pid_temp->err_max&&speed_pid_temp->err_max!=0)
{
speed_pid_temp->err=speed_pid_temp->err_max;
}
if(speed_pid_temp->err<=speed_pid_temp->err_min&&speed_pid_temp->err_min!=0)
{
speed_pid_temp->err=speed_pid_temp->err_min;
}
tmpUp = speed_pid_temp->err*speed_pid_temp->P_Gain_q12;
tmpUp>>=12;
speed_pid_temp->I_Sum_temp += speed_pid_temp->err;
if(speed_pid_temp->I_Sum_temp> speed_pid_temp->I_Max) speed_pid_temp->I_Sum_temp = speed_pid_temp->I_Max;
if(speed_pid_temp->I_Sum_temp< speed_pid_temp->I_Min) speed_pid_temp->I_Sum_temp = speed_pid_temp->I_Min;
speed_pid_temp->I_Sum = speed_pid_temp->I_Sum_temp;
speed_pid_temp->I_Sum*=speed_pid_temp->I_Gain_q12;
speed_pid_temp->I_Sum>>=12;
tmpErr = speed_pid_temp->err-speed_pid_temp->err_old;
tmpUd = tmpErr*speed_pid_temp->D_Gain_q12;
tmpUd>>=12;
speed_pid_temp->result_out=(tmpUp+speed_pid_temp->I_Sum+tmpUd);
if(speed_pid_temp->result_out > speed_pid_temp->Max_Output) speed_pid_temp->result_out = speed_pid_temp->Max_Output;
if(speed_pid_temp->result_out < speed_pid_temp->Min_Output) speed_pid_temp->result_out = speed_pid_temp->Min_Output;
}
