52 lines
1.3 KiB
C
52 lines
1.3 KiB
C
#include <stdint.h>
|
|
#include <ti_msp_dl_config.h>
|
|
#define limit 5.0
|
|
#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;
|
|
uint32_t Timestamp = DL_SYSTICK_getValue();
|
|
if(Timestamp < Timestamp_Last)
|
|
{
|
|
Ts = (float)(Timestamp_Last - Timestamp) / 80 * 1e-6;
|
|
}
|
|
else
|
|
{
|
|
Ts = (0xFFFFFF - Timestamp + Timestamp_Last) / 80 * 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;
|
|
}
|
|
|