struct PID_STRUCT { float Kp; fload Ki; fload Kd; fload X; float Intergrator; };
void PID_Initilize(struct PID_STRUCT* pid, float Kp, float Ki, float Kd) { pid->Kp = Kp; pid->Ki = Ki; pid->Kd = Kd; pid->X = 0; pid->Intergrator = 0; }
float PID_RUN(struct PID_STRUCT* pid, float input) { pid->Intergrator += pid->Ki * input;
// float output = pid->Kp * input + pid->Intergrator + pid->Kd*(input - pid->X); becomes: float output = (pid->Kp + pid->Kd) * input + pid->Intergrator - pid->Kd * pid->X; //(pid->Kp + pid->Kd) can be merged.
pid->X = input; return output; }
程序完全可以优化,比如用 16-bit int 代替 float. |