handcode_pid.c

来自「这是一个电机PID控制的程序」· C语言 代码 · 共 69 行

#include "handcode_pid.h"

struct
{
    float K;
    float Ti;
    float Td;
    float Ts;
    float b;
    float N;
    float state_I;
    float state_y;
    float state_Dk;
} pid_params;

void
pid_controller_init()
{
    pid_params.K = 0.1F;
    pid_params.Ti = 0.01F;
    pid_params.Td = 0.00001F;
    pid_params.Ts = 0.0001F;
    pid_params.b = 0.7F;
    pid_params.N = 4.0F;
    pid_params.state_I = 0.0F;
    pid_params.state_y = 0.0F;
    pid_params.state_Dk = 0.0F;
}

void
pid_controller_step(float y, float uc, float *u_out)
{
    float e;
    float Pk;
    float Ik;
    float Dk;
    float Kd;
    float Kdd;
    float tmp;

    /* Calculate the proportional path
     */
    Pk = pid_params.K * (uc * pid_params.b - y);

    /* Calculate the integral path
     */
    Ik = pid_params.state_I;
    e = (uc - y);
    pid_params.state_I = (e*pid_params.K*pid_params.Ts/pid_params.Ti) + pid_params.state_I;

    /* Calculate the derivative path
     */
    Kd = pid_params.K * pid_params.Td * pid_params.N /
         (pid_params.Td + pid_params.N * pid_params.Ts);  
    Kdd = pid_params.Td /
         (pid_params.Td + pid_params.N * pid_params.Ts);  
    tmp = Kd * (y - pid_params.state_y);

    Dk = pid_params.state_Dk * Kdd - tmp;

    pid_params.state_y = y;
    pid_params.state_Dk = Dk;

    /* Sum the parts to the final output
     */
    u_out[0] = Pk + Ik + Dk;
}