pid算法Ｃ语言实现

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

pid.h

#ifndef __PID_H
#define __PID_Htypedef 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 -100void 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;elseret = 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 <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#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