97 lines
2.0 KiB
C
97 lines
2.0 KiB
C
#include "PID.h"
|
||
|
||
float SPEED_P,SPEED_I,SPEED_D=0;
|
||
|
||
double SSumError=0,SPrevError=0,SLastError=0;
|
||
int SdError=0,SError=0;
|
||
|
||
void PID_init(void)
|
||
{
|
||
SError=0;SSumError=0;SLastError=0;
|
||
|
||
|
||
SPEED_P=(float)SP/1000;
|
||
SPEED_I=(float)SI/1000;
|
||
SPEED_D=(float)SD/1000;
|
||
|
||
// SSumError=OutPwmValue;
|
||
// SSumError=SSumError/SPEED_I;
|
||
}
|
||
|
||
void PIDcompute(unsigned int Target, unsigned int Real)
|
||
{
|
||
float j = 0.0f;
|
||
|
||
if(Target * MPolePairs < 600) Target = 600 / MPolePairs;
|
||
|
||
SError = (Target - Real);
|
||
|
||
// 改进的积分项处理
|
||
float temp_integral = SSumError + SError;
|
||
float integral_limit = PWM_PERIOD / SPEED_I;
|
||
|
||
// 积分限幅
|
||
if(temp_integral > integral_limit) {
|
||
SSumError = integral_limit;
|
||
} else if(temp_integral < -integral_limit) {
|
||
SSumError = -integral_limit;
|
||
} else {
|
||
SSumError = temp_integral;
|
||
}
|
||
|
||
// 微分项计算
|
||
SdError = SError - SLastError;
|
||
SLastError = SError;
|
||
|
||
// PID输出计算
|
||
j = SPEED_P * SError + SPEED_I * SSumError + SPEED_D * SdError;
|
||
|
||
// 输出限幅
|
||
if(j > PWM_PERIOD) {
|
||
OutPwmValue = PWM_PERIOD;
|
||
} else if(j < 1) {
|
||
OutPwmValue = 1;
|
||
} else {
|
||
OutPwmValue = (int)j;
|
||
}
|
||
|
||
UPPWM();
|
||
}
|
||
/*
|
||
void PIDcompute(unsigned int Target,unsigned int Real)
|
||
{
|
||
float j=0.0,i;
|
||
|
||
|
||
if(Target*MPolePairs<600)Target=600/MPolePairs; //限速，4对极下150RPM
|
||
|
||
SError =(Target-Real); //本次偏差
|
||
|
||
if(SSumError<500)SSumError=500;
|
||
else if(SSumError*SPEED_I>PWM_PERIOD)
|
||
{
|
||
SSumError=PWM_PERIOD;
|
||
SSumError=SSumError/SPEED_I;
|
||
} // 输出到最大值后，不再累计偏差，抗积分饱各
|
||
else SSumError +=SError;
|
||
|
||
|
||
SdError=SError-SLastError; //微分项，偏差的变化
|
||
SLastError=SError; //记录本次偏差，用于下次计算
|
||
|
||
i=SPEED_P;
|
||
j=SError*i;
|
||
i=SPEED_I;
|
||
j=j+SSumError*i;
|
||
i=SPEED_D;
|
||
j=j+SdError*i;
|
||
|
||
if(j>PWM_PERIOD)OutPwmValue=PWM_PERIOD;
|
||
else if(j<1)OutPwmValue=1;
|
||
else OutPwmValue=j;
|
||
|
||
|
||
UPPWM();
|
||
}
|
||
*/
|