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; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
dk1=pid.En-pid.En_1; //<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD>ƫ<EFBFBD><C6AB>֮<EFBFBD><D6AE>
|
|||
|
|
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; //<2F><><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
pid.currpwm+=pid.Dout; //<2F><><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
|
|||
|
|
|
|||
|
|
}
|