46 lines
1.1 KiB
C
46 lines
1.1 KiB
C
#include <stdint.h>
|
|
|
|
#define limit 6.3
|
|
#define Output_ramp 10000
|
|
|
|
//限幅
|
|
float _constrain(float amt, float low, float high)
|
|
{
|
|
return ((amt<low)?(low):((amt)>(high)?(high):(amt)));
|
|
}
|
|
|
|
unsigned long Timestamp_Last = 0.0;
|
|
float Last_Error = 0.0;
|
|
float Last_intergration = 0.0;
|
|
float Last_Output = 0.0;
|
|
float PID_Controller(float Kp, float Ki, float Kd, float Error)
|
|
{
|
|
float Ts = 0.0;
|
|
//TODO
|
|
uint32_t Timestamp = 0;
|
|
//uint32_t Timestamp = SysTick->VAL;
|
|
if(Timestamp < Timestamp_Last) Ts = (float)(Timestamp_Last - Timestamp)/9*1e-6;
|
|
else
|
|
Ts = (0xFFFFFF - Timestamp + Timestamp_Last)/9*1e-6;
|
|
|
|
if(Ts<=0 || Ts > 0.05f) Ts = 0.001;
|
|
|
|
float proportion = Kp * Error;//P环
|
|
|
|
float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环
|
|
intergration = _constrain(intergration, -limit, limit);
|
|
|
|
float differential = Kd * (Error - Last_Error)/Ts;//D环
|
|
|
|
float Output = proportion + intergration + differential;
|
|
Output = _constrain(Output, -limit, limit);
|
|
|
|
Last_Error = Error;
|
|
Last_intergration = intergration;
|
|
Last_Output = Output;
|
|
Timestamp_Last = Timestamp;
|
|
|
|
return Output;
|
|
}
|
|
|