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);
|
|||
|
|
|
|||
|
|
// <20>Ľ<EFBFBD><C4BD>Ļ<EFBFBD><C4BB><EFBFBD><EFBFBD><EFBFBD><EEB4A6>
|
|||
|
|
float temp_integral = SSumError + SError;
|
|||
|
|
float integral_limit = PWM_PERIOD / SPEED_I;
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(temp_integral > integral_limit) {
|
|||
|
|
SSumError = integral_limit;
|
|||
|
|
} else if(temp_integral < -integral_limit) {
|
|||
|
|
SSumError = -integral_limit;
|
|||
|
|
} else {
|
|||
|
|
SSumError = temp_integral;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <><CEA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
SdError = SError - SLastError;
|
|||
|
|
SLastError = SError;
|
|||
|
|
|
|||
|
|
// PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
j = SPEED_P * SError + SPEED_I * SSumError + SPEED_D * SdError;
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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; //<2F><><EFBFBD>٣<EFBFBD>4<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>150RPM
|
|||
|
|
|
|||
|
|
SError =(Target-Real); //<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
|
|||
|
|
|
|||
|
|
if(SSumError<500)SSumError=500;
|
|||
|
|
else if(SSumError*SPEED_I>PWM_PERIOD)
|
|||
|
|
{
|
|||
|
|
SSumError=PWM_PERIOD;
|
|||
|
|
SSumError=SSumError/SPEED_I;
|
|||
|
|
} // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><F3A3ACB2><EFBFBD><EFBFBD>ۼ<EFBFBD>ƫ<EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>
|
|||
|
|
else SSumError +=SError;
|
|||
|
|
|
|||
|
|
|
|||
|
|
SdError=SError-SLastError; //<><CEA2><EFBFBD>ƫ<EEA3AC><C6AB><EFBFBD>ı仯
|
|||
|
|
SLastError=SError; //<2F><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD>´μ<C2B4><CEBC><EFBFBD>
|
|||
|
|
|
|||
|
|
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();
|
|||
|
|
}
|
|||
|
|
*/
|