pid算法Ｃ语言实现

理论我就不多说了，网都已经很多了，但能直接看到效果的确不多。这里我就提供一个Ｃ语言实现的可以看到效果的实际例程。

pid.h

#ifndef __PID_H
#define __PID_H

typedef struct pid {
  int error_last;
  int error_last_last;
  float kp;
  float ki;
  float kd;
  // 仅位置式PID使用
  long integral;
  int target;
  int current;
  int times;
}ppid_t;

#define KP  0.1
#define KI  0.8
#define KD  0.1
#define PID_MAX 100
#define PID_MIN -100

void pid_init(int target, int init);
void pid_set_target(int target);
void pid_set_proportion(float kp, float ki, float kd);
int pid_get_change_value(int current);
int pid_get_point_value(int value);
int pid_get_times();

#endif

pid.c

#include "pid.h"
static ppid_t pid;

/*
* @brife   : pid结构初始化
* @param   : target 目标值
* @param   : init 初始值
* @return  : 无
*/
void pid_init(int target, int init)
{
  pid.error_last = target - init;
  pid.error_last_last = target - init;
  pid.kp = KP;
  pid.ki = KI;
  pid.kd = KD;
  pid.integral = 0;
  pid.target = target;
  pid.current = init;
  pid.times = 0;
}

/*
* @brife  : 设置pid目标值
* @param  : target 目标值
* @return : 无
*/
void pid_set_target(int target)
{
  pid.target = target;
}

/*
* @brife  : 设置pid比例，微分，积分比例大小
* @param  : kp 比例值
* @param  : ki 积分比例
* @param  : kd 微分比例
* @return : 无
*/
void pid_set_proportion(float kp, float ki, float kd)
{
  pid.kp = kp;
  pid.ki = ki;
  pid.kd = kd;
}

/*
* @brife  : 增量式pid, 获取下次改变值
* @param  : current 当前值
* @return : 改变值，可正，可负
*/
int pid_get_change_value(int current)
{
  int error = 0;
  int p_out, i_out, d_out;
  int ret;

  error = pid.target - current;
  p_out = pid.kp * (error - pid.error_last);
  i_out = pid.ki * error;
  d_out = pid.kd * (error - 2 * pid.error_last + pid.error_last_last);
  ret = p_out + i_out + d_out;
  if(ret > PID_MAX)
    ret = PID_MAX;
  if(ret < PID_MIN)
    ret = PID_MIN;
  pid.error_last_last = pid.error_last;
  pid.error_last = error;
  pid.times += 1;
  return ret;
}

/*
* @brife  : 位置式pid获取下次预测值
* @param  : 当前值
* @return : 下次预测值
*/
int pid_get_point_value(int value)
{
  int ret = 0;
  int p_out, d_out;
  long i_out = 0;
  int error = 0;

  error = pid.target - value;
  pid.integral += error;
  if(pid.integral > (1 << 30))
    pid.integral = 0;
  p_out = pid.kp * error;
  i_out = pid.ki * pid.integral;
  d_out = pid.kd * (error - pid.error_last);
  if((p_out + i_out + d_out - value) < PID_MIN)
    ret = value + PID_MIN;
  else if((p_out + i_out + d_out - value) > PID_MAX)
    ret = value + PID_MAX;
  else
    ret = p_out + i_out + d_out;
  pid.error_last = error;
  pid.times += 1;

  return ret;
}

/*
* @brife  : pid 调整次数
* @param  : 无
* @return : pid调整次数
*/
int pid_get_times(){
  return pid.times;
}

main.c

#include 
#include 
#include 
#include "pid.h"

int main(int argc, char **argv)
{
  int target, current;

  if(argc < 3){
    fprintf(stderr,"Usage: ./main target current\n");
    exit(1);
  }
  target = atoi(argv[1]);
  current = atoi(argv[2]);

  pid_init(target, current);
  while(1){
    current += pid_get_change_value(current);
    printf("%d: current %d\n", pid_get_times(), current);
    sleep(1);
  }
  exit(0);
}

编译与运行

gcc -o main main.c pid.c

./main 500 123

下面是运行结果。可以看到越来越近500，但到500后有又到了501，可以通过调整kp, ki, kd改善，但也没有那个必要，实际工程中多１格，少１格一般没有太大影响。前期以100的进度增加，因为pid.c中做了限制。这是考虑到实际工程中的设定值斜率往往不能太大。

1: current 220
2: current 320
3: current 420
4: current 474
5: current 493
6: current 500
7: current 501
8: current 501
9: current 501
10: current 501
11: current 501
12: current 501
13: current 501
14: current 501
15: current 501