/*
* driver_pid.c
*
* Created on: 2022-11-18
* Author: chipon
*/
#include "system_init.h"
#include "hardware_init.h"
#include "application_init.h"
PID_ID_TypeDef PID_ID; // 提供磁通的电流分量Id
PID_IQ_TypeDef PID_IQ; // 提供转矩的电流分量Iq
w_PIParm PIParm_W; // 转速、角速度
void PIDInit_D(void)
{
PID_ID.Id_Kp = _IQ12(USER_ID_KP); //DKP;
PID_ID.Id_Ki = _IQ12(USER_ID_KI); //DKI;
PID_ID.Id_OutMax = IQ15(USER_ID_OUTMAX); //DOUTMAX;
PID_ID.Id_OutMin = -PID_ID.Id_OutMax;
PID_ID.Id_Sum = 0;
PID_ID.Id_Out = 0;
}
void PIDInit_Q(void)
{
PID_IQ.Iq_Kp = _IQ12(USER_IQ_KP); //QKP;
PID_IQ.Iq_Ki = _IQ12(USER_IQ_KI); //QKI;
PID_IQ.Iq_Kd = _IQ12(0.0);
PID_IQ.Iq_Err = 0;
PID_IQ.Iq_Err_Last1 = 0;
PID_IQ.Iq_Err_Err = 0;
PID_IQ.Iq_Out = _IQ15(0.0);
PID_IQ.Iq_OutMax = IQ15(USER_IQ_OUTMAX); //QOUTMAX;
PID_IQ.Iq_OutMin = -PID_IQ.Iq_OutMax;
}
void PIDInit_W(void)
{
PIParm_W.w_Kp = IQ15(USER_SPD_KP); //WKP;
PIParm_W.w_Ki = IQ15(USER_SPD_KI); //WKI;
PIParm_W.w_OutMax = IQ15(USER_SPD_OUTMAX); //WOUTMAX;
PIParm_W.w_OutMin = -PIParm_W.w_OutMax;
PIParm_W.w_Sum = 0;
PIParm_W.w_Out = 0;
}
//电流环D轴PI控制
void PI_Control_D( PID_ID_TypeDef *dParm)
{
int32_t U;
U = (dParm->Id_Sum + dParm->Id_Kp*dParm->Id_Err)>>12;
if(U > dParm->Id_OutMax)
{
U = dParm->Id_OutMax;
}
else if(U < dParm->Id_OutMin)
{
U = dParm->Id_OutMin;
}
else
{
dParm->Id_Out = U;
}
dParm->Id_Sum = dParm->Id_Sum + dParm->Id_Ki*dParm->Id_Err;
}
//电流环Q轴PI控制
/* -------------------------------------------------------------------------------------------------
Function Name : PIDControl
Description : 增量式PID：u(k)=Kp * e(k-1)+Ki *e(k) +Kd *(e(k)-2e(k-1)+e(k-2))；
Date : 2022-11-22
Parameter : PID: [输入/出]
** Ref: [输入/出]
** Cur: [输入/出]
------------------------------------------------------------------------------------------------- */
int16_t PIDControl_Q(PID_IQ_TypeDef * PID, int16_t Ref, int16_t Cur)
{
int32_t Kp_Out, Ki_Out, PID_Out,PID_Out_Last;
if (!PID->Iq_Err)
{
PID->Iq_Err = Ref - Cur; // 初始化PID当前偏差
PID->Iq_Err_Err = PID->Iq_Err - PID->Iq_Err_Last1; // 初始化PID上次偏差和上上次偏差之差
PID->Iq_Err_Last1 = Ref - Cur; // 初始化PID上次偏差
PID->Iq_Err_Err_Last1 = PID->Iq_Err_Err;
}
else
{
PID->Iq_Err_Last1 = PID->Iq_Err; // 保存PID上次偏差
PID->Iq_Err = Ref - Cur; // 计算PID当前偏差
PID->Iq_Err_Err = PID->Iq_Err - PID->Iq_Err_Last1; // 计算PID上次偏差和上上次偏差之差
}
Kp_Out = ((int32_t)PID->Iq_Kp * (int32_t)PID->Iq_Err_Err)>>12;
Ki_Out = ((int32_t)PID->Iq_Ki * (int32_t)PID->Iq_Err)>>12;
PID_Out = PID->Iq_Out;
PID_Out += Kp_Out + Ki_Out;
if (PID_Out > PID->Iq_OutMax)
{
PID_Out = PID->Iq_OutMax; // PID最高输出
}
if (PID_Out < PID->Iq_OutMin)
{
PID_Out = PID->Iq_OutMin; // PID最低输出
}
PID->Iq_Out = PID_Out;
return PID->Iq_Out;
}
//速度环PI控制
void PI_Control_W(w_PIParm *wParm)
{
int32_t U;
U = (wParm->w_Sum + wParm->w_Kp*wParm->w_Err)>>15;
if(U > wParm->w_OutMax)
{
U = wParm->w_OutMax;
}
else if(U < wParm->w_OutMin)
{
U = wParm->w_OutMin;
}
else
{
wParm->w_Out = U;
}
wParm->w_Sum = wParm->w_Sum + wParm->w_Ki*wParm->w_Err;
}
uint32_t SQRT_VqMax(uint32_t BeSqrt)
{
uint32_t ToLeft = 16;
uint32_t hStag = 0;
uint32_t uStag = 0;
for(ToLeft; ToLeft>0; ToLeft--)
{
hStag = uStag + (1<<(ToLeft-1));
if((hStag*hStag) <= BeSqrt)
uStag += (1<<(ToLeft-1));
}
return uStag;
}
- 1
- 2
前往页