142 lines
2.2 KiB
C
142 lines
2.2 KiB
C
#include "pid.h"
|
|
|
|
PID pid;
|
|
extern u16 pidcalcms;
|
|
void PIDParament_Init() //
|
|
{
|
|
pid.set = 180;
|
|
pid.currpwm= 0;
|
|
pid.pwm_cycle= 90; //
|
|
// pid.pid_P=8;
|
|
// pid.pid_I=4;
|
|
// pid.pid_D=5;
|
|
pid.calc_cycle=90;
|
|
|
|
pid.Td= 0.3; // 2000
|
|
pid.Ti= 0; // 4000
|
|
pid.Kp= 10; //20; // 5
|
|
pid.Tsam=50;
|
|
|
|
pid.Sen_max = 10;
|
|
}
|
|
|
|
/*
|
|
void pid_calc() //pid??
|
|
{
|
|
//int dk;
|
|
|
|
|
|
short PID_out;
|
|
|
|
if(pidcalcms<pid.calc_cycle) return ;
|
|
|
|
pid.Ek=pid.set-pid.curr;//????(????-????)
|
|
PID_out=((pid.pid_P)*((pid.Ek)-(pid.Ek_1)))+ \
|
|
((pid.pid_I)*(pid.Ek))+ \
|
|
((pid.pid_D)*((pid.Ek)-(2*(pid.Ek_1))+(pid.Ek_2))); //??????
|
|
pid.currpwm+=PID_out;
|
|
pid.Ek_2=pid.Ek_1; //???????
|
|
pid.Ek_1=pid.Ek; //??????
|
|
|
|
if(pid.currpwm>pid.pwm_cycle) //1000 //1s??PWM??
|
|
{
|
|
pid.currpwm=pid.pwm_cycle; //????????1000???????
|
|
}
|
|
if(pid.currpwm<=0) //??????0??0??(????)
|
|
{
|
|
pid.currpwm=0;//
|
|
} //PID????
|
|
pidcalcms=0;
|
|
}
|
|
*/
|
|
void pid_calc() //pid??
|
|
{
|
|
float dk1;float dk2;
|
|
float t1,t2,t3;
|
|
|
|
//short PID_out;
|
|
|
|
if(pidcalcms<pid.Tsam)
|
|
|
|
return ; //
|
|
|
|
|
|
|
|
pid.En=pid.set-pid.curr; //本次误差
|
|
dk1=pid.En-pid.En_1; //本次偏差与上次偏差之差
|
|
dk2=pid.En-2*pid.En_1+pid.En_2;
|
|
|
|
t1=pid.Kp*dk1;
|
|
|
|
t2=(pid.Kp*pid.Tsam)/pid.Ti;
|
|
t2=t2*pid.En;
|
|
|
|
t3=(pid.Kp*pid.Td)/pid.Tsam;
|
|
t3=t3*dk2;
|
|
|
|
pid.Dout=t1+t2+t3; //本次应该输出的增量
|
|
|
|
pid.currpwm+=pid.Dout; //本次应该输出的PWM
|
|
if(pid.currpwm>pid.pwm_cycle + 3)
|
|
{
|
|
pid.currpwm=pid.pwm_cycle;
|
|
}
|
|
|
|
//printf("Dout: %f t1 : %f %f %f\r\n",pid.Dout,t1,pid.En,dk1);
|
|
|
|
if(pid.currpwm<0)
|
|
{
|
|
pid.currpwm=0;
|
|
}
|
|
pid.En_2=pid.En_1;
|
|
pid.En_1=pid.En;
|
|
|
|
pidcalcms=0;
|
|
}
|
|
|
|
u8 pid_our_control( u16 currentTemp )
|
|
{
|
|
|
|
int pwm;
|
|
if(pidcalcms<pid.Tsam)
|
|
{
|
|
|
|
|
|
|
|
}
|
|
else
|
|
{
|
|
pid.En = pid.set - currentTemp ;
|
|
|
|
pid.Sen += pid.En;
|
|
|
|
|
|
if( pid.Sen > pid.Sen_max )
|
|
{
|
|
pid.Sen = pid.Sen_max;
|
|
}
|
|
else if ( pid.Sen < pid.Sen_min)
|
|
{
|
|
pid.Sen = pid.Sen_min;
|
|
}
|
|
|
|
|
|
pid.currpwm = pid.Kp * pid.En + pid.Ti * pid.Sen + pid.Td * ( pid.En - pid.last_En );
|
|
|
|
if( pid.currpwm > pid.pwm_cycle )
|
|
{
|
|
pid.currpwm = pid.pwm_cycle;
|
|
}
|
|
else if ( pid.currpwm < 0 )
|
|
{
|
|
pid.currpwm = 0 ;
|
|
}
|
|
|
|
pid.last_En = pid.En;
|
|
|
|
}
|
|
|
|
return pid.currpwm;
|
|
|
|
}
|