C语言增量式PID通用算法

只看该作者 · 2023-2-27 10:52

C语言增量式PID通用算法

控制算法中PID使用的非常广泛，但是在网上找代码的时候发现好多代码都不够通用，需要自己改好多东西，而且当一个项目需要使用多个PID控制器时也颇为麻烦，这里设计一个通用的增量式PID算法，可以直接使用，不用更改什么东西，顶多改一下初始化时候的几个参数限制范围。

该算法的思想是设计一个PID控制器类，使用的时候直接实例化一个PID对象即可，也可以实例化多个，每个都是独立的PID控制器。

该库留出来PID控制器的所用有接口，对于调试来说非常方便，不需要外部再定义一堆函数来调用。该库实现了PID参数设置与修改，控制器控制函数接口，可以说是十分方便易用，且完全做到了代码隔离。这其实是面向对象的思维模式，相当于实现一个PID控制的类以及方法，使用时只需使用该类来定义一个对象即可。

只看该作者 · 2023-2-27 10:53

头文件：
#ifndef _PID_H
#define _PID_H

typedef struct pid_structure {
float kp, ki, kd;
float kp_max, kp_min;
float ki_max, ki_min;
float kd_max, kd_min;
float err_k, err_k1, err_k2;
float u_k, u_k1;
float delta_u_k;
} PidObj;

typedef enum {
KP,
KI,
KD
} PidParam;

void pid_init(PidObj* obj,
            float kp, float ki, float kd,
            float kp_min, float kp_max,
            float ki_min, float ki_max,
            float kd_min, float kd_max);
void pid_set_param(PidObj* obj, float kp, float ki, float kd);
void pid_get_param(PidObj* obj, float* kp, float* ki, float* kd);
void pid_set_param_limit(PidObj* obj, PidParam param, float min, float max);
void pid_get_param_limit(PidObj* obj, PidParam param, float* min, float* max);
float pid_control(PidObj* obj, float err);

#endif

只看该作者 · 2023-2-27 10:54

C文件:
#include "./pid.h"

void pid_init(PidObj* obj,
            float kp, float ki, float kd,
            float kp_min, float kp_max,
            float ki_min, float ki_max,
            float kd_min, float kd_max)
{
obj->delta_u_k  = 0.0f;
obj->err_k2    = 0.0f;
obj->err_k1    = 0.0f;
obj->err_k    = 0.0f;
obj->u_k1    = 0.0f;
obj->u_k       = 0.0f;
obj->kp       = kp;
obj->ki       = ki;
obj->kd       = kd;
obj->kp_min    = kp_min;
obj->kp_max    = kp_max;
obj->ki_min    = ki_min;
obj->ki_max    = ki_max;
obj->kd_min    = kd_min;
obj->kd_max    = kd_max;
}

void pid_set_param(PidObj *obj, float kp, float ki, float kd)
{
obj->kp = (obj->kp_min == 0 && obj->kp_max == 0) ? kp : \
            ((kp > obj->kp_max) ? obj->kp_max : \
            ((kp < obj->kp_min) ? obj->kp_min : kp));

obj->ki = (obj->ki_min == 0 && obj->ki_max == 0) ? ki : \
            ((ki > obj->ki_max) ? obj->ki_max : \
            ((ki < obj->ki_min) ? obj->ki_min : ki));

obj->kd = (obj->kd_min == 0 && obj->kd_max == 0) ? kd : \
            ((kd > obj->kd_max) ? obj->kd_max : \
            ((kd < obj->kd_min) ? obj->kd_min : kd));
}

void pid_get_param(PidObj *obj, float *kp, float *ki, float *kd)
{
*kp = obj->kp;
*ki = obj->ki;
*kd = obj->kd;
}

void pid_set_param_limit(PidObj *obj, PidParam param, float min, float max)
{
switch (param)
{
case KP:
      obj->kp_min = min;
      obj->kp_max = max;
      break;
case KI:
      obj->ki_min = min;
      obj->ki_max = max;
      break;
case KD:
      obj->kd_min = min;
      obj->kd_max = max;
      break;
}
}

void pid_get_param_limit(PidObj *obj, PidParam param, float *min, float *max)
{
switch (param)
{
case KP:
      *min = obj->kp_min;
      *max = obj->kp_max;
      break;
case KI:
      *min = obj->ki_min;
      *max = obj->ki_max;
      break;
case KD:
      *min = obj->kd_min;
      *max = obj->kd_max;
      break;
}
}

float pid_control(PidObj *obj, float err)
{
obj->err_k2 = obj->err_k1;
obj->err_k1 = obj->err_k;
obj->err_k = err;
obj->u_k1 = obj->u_k;

obj->delta_u_k = obj->kp * (obj->err_k - obj->err_k1) +
                  obj->ki * obj->err_k +
                  obj->kd * (obj->err_k - 2 * obj->err_k1 + obj->err_k2);

obj->u_k = obj->u_k1 + obj->delta_u_k;
return obj->u_k;
}