PID算法C语言实现

PID算法增量式与位置式C语言实现

---

【头文件】

#ifndef PID_H_
#define PID_H_

typedef struct
{
    float kp;           // 比例系数
    float ki;           // 积分系数
    float kd;           // 微分系数
    float err_last;     // 上次误差
    float err_sum;      // 误差累计
    float result;
}pid_pos_typedef;
void pid_pos_init(pid_pos_typedef* pid, float kp, float ki, float kd);
float pid_pos_calc(pid_pos_typedef* pid, float currVal, float objVal);


typedef struct
{
    float kp;           // 比例系数
    float ki;           // 积分系数
    float kd;           // 微分系数
    float err_last;     // 上一次的误差
    float err_pree;     // 上二次的误差
    float result;
}pid_delta_typedef;
void pid_delta_init(pid_delta_typedef* pid, float kp, float ki, float kd);
float pid_delta_calc(pid_delta_typedef* pid, float currVal, float objVal);

#endif /* PID_H_ */

【源文件】

//
// Created by Administrator on 2019/10/24 0024.
//

#include "pid.h"

// 位置式pid算法初始化
void pid_pos_init(pid_pos_typedef* pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->result = 0;
    pid->err_last = 0;
    pid->err_sum = 0;
}

// 位置式pid算法计算
float pid_pos_calc(pid_pos_typedef* pid, float currVal, float objVal)
{
    float err_c = objVal - currVal;       // 当前误差
    pid->err_sum += err_c;                  // 误差累计
    pid->result = pid->kp * err_c + pid->ki * pid->err_sum + pid->kd * (err_c - pid->err_last);
    pid->err_last = err_c;
    return pid->result;
}

// 增量式pid算法初始化
void pid_delta_init(pid_delta_typedef* pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->result = 0;
    pid->err_last = 0;
    pid->err_pree = 0;
}

// 增量式算法计算
float pid_delta_calc(pid_delta_typedef* pid, float currVal, float objVal)
{
    float err_c;            // 当前误差
    float err_p;            // p误差
    float err_i;            // i误差
    float err_d;            // d误差
    float increment;        // 增量

    err_c = objVal - currVal;
    err_p = err_c - pid->err_last;
    err_i = err_c;
    err_d = err_c - 2 * pid->err_last + pid->err_pree;
    increment = pid->kp * err_p + pid->ki * err_i + pid->kd * err_d;

    pid->err_pree = pid->err_last;
    pid->err_last = err_c;
    pid->result += increment;
    return pid->result;
}

---

测试

int main(void)
{
  uint32_t i = 0;
  float currVal = 0;        // 当前值
  float objVal = 10;        // 目标值
  pid_delta_typedef pid_delta;
  pid_delta_init(&pid_delta, 0.2, 0.001, 0.0001);

  while(1)
  {
    i++;
    currVal = pid_delta_calc(&pid_delta, currVal, objVal);
    printf("[%d]    currVal:%f \n", i, currVal);

    if(currVal >= 9.999)
      break;
  }

  printf("------ end. \n");
  return 0;
}

---

Python版增量式PID

from matplotlib import pyplot as plt


class PID:
    __err_last = 0.0
    __err_pree = 0.0
    __ki = 0.0
    __kp = 0.0
    __kd = 0.0
    __result = 0.0

    def __init__(self, ki, kp, kd):
        self.__ki = ki
        self.__kp = kp
        self.__kd = kd

    def calc(self, objVal, currVal):
        err_c = objVal - currVal
        err_p = err_c - self.__err_last
        err_i = err_c
        err_d = err_c - 2 * self.__err_last + self.__err_pree
        increment = self.__kp * err_p + self.__ki * err_i + self.__kd * err_d

        self.__err_pree = self.__err_last
        self.__err_last = err_c
        self.__result += increment
        return self.__result

    def setKP(self, kp):
        self.__kp = kp

    def setKI(self, ki):
        self.__ki = ki

    def setKD(self, kd):
        self.__kd = kd


if __name__ == '__main__':
    index = 0
    currval = 0.0

    pid = PID(0.2, 0.001, 0.0001)

    array = []
    array.append(currval)

    while True:
        if currval >= 99.999:
            break
        index += 1
        currval = pid.calc(100, currval)
        array.append(currval)
        print("[%d]    curr:%f      " % (index, currval))

    plt.figure()
    plt.xlabel('count')
    plt.ylabel('value')
    plt.plot(array)
    plt.show()

从0到100实验效果：

---

Python版位置式PID

from matplotlib import pyplot as plt


class PID:
    __err_last = 0.0
    __err_sum = 0.0
    __ki = 0.0
    __kp = 0.0
    __kd = 0.0

    def __init__(self, ki, kp, kd):
        self.__ki = ki
        self.__kp = kp
        self.__kd = kd

    def calc(self, objVal, currVal):
        err = objVal - currVal
        self.__err_sum += err
        result = self.__kp * err + self.__ki * self.__err_sum + self.__kd * (err - self.__err_last)
        self.__err_last = err
        return result

    def setKP(self, kp):
        self.__kp = kp

    def setKI(self, ki):
        self.__ki = ki

    def setKD(self, kd):
        self.__kd = kd


if __name__ == '__main__':
    index = 0
    currval = 0.0

    pid = PID(0.2, 0.001, 0.0001)

    array = []
    array.append(currval)

    while True:
        if currval >= 99.999:
            break

        index += 1
        currval = pid.calc(100, currval)
        array.append(currval)
        print("[%d]    curr:%f      " % (index, currval))

    plt.figure()
    plt.xlabel('count')
    plt.ylabel('value')
    plt.plot(array)
    plt.show()

从0加到100实验效果：

---

ends…