#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;
}