前面我们已经知道位置型PID和增量型PID的数学表达式，我们根据这些表达式就可以实现基本的PID的C语言编程了！下面我们开始C语言的编写。
第一步：创建一个PID各项参数的变量结构体。
第二步：PID参数的初始化函数。
这里以后我们主要就是对Kp、Ki、Kd的不断调节来优化控制效果！
第三步：PID算法的计算函数。
注意：这里没有很严格的保证PID的算法精确，只是对公式的一种直接实现，以后我们当然要慢慢的优化！
上面就已经结束了对PID算法的设计，下面就开始输出测试了：
代码如下：
下面是经过1000次PID控制之后的结果：因为数据很多，我只给出了一部分