一、 PID公式
二、 PID应用的必要性
1. 四驱小车运动
2. 产生多种运动的原因
小车的4个电机，减速箱以及车轮在物理层面上存在误差，虽然给4个轮子通了相同pwm（电流），但是却没办法保证运行时车子的4个轮子的转速相同。
我们只知道通过改变电流让电机旋转起来，却不知道轮子的实际转速，所以没法保证小车跑直线。这就是开环控制。
3. 解决方案分析
小车跑偏就是因为小车现在的控制系统是开环的，只能用pwm控制电机让它旋转，并不知道轮子的实际速度是多少。
只有知道了方向是否发生了偏转，才能进一步对小车的运动方向进行控制调整。例如，往左偏了，我们就把右边的轮子速度降低一点。如果往右边偏了，我们就把边的轮子速度升高一点。
4. 解决方案
引入反馈信号：如果我们能得知车子的运动方向，就可以对车子的方向加以纠正，控制小车跑直线（闭环控制）。
（1）给小车安装电子陀螺仪
用它来测量小车运行的方向信号，从而控制单片机pwm的输出值（input）去修正电机的工作电流，从而修改轮子的实际速度（output），从而纠正小车的跑偏的问题，从而使用PID技术实现方向对小车方向的闭环控制。
闭环比开环多了反馈信号feedback。我们这里的feedback信号选用了电子陀螺仪的偏转角（yaw值）。根据闭环控制，设计一个小车的实际控制方案，例如，保持左边的轮子速度不变，当车子向右发生偏转时，我就加快右边轮子的转速。当车子向左偏的时候，我就降低右边轮子的转速。
（2）确定反馈信号量和控制量之间的比例关系
输入信号和输出信号存在一种比例关系，即传感器的值yaw和驱动电机的pwm的值存在一种比例关系(比例P)。方向偏的越多，那么右轮的pwm应该越大，方向偏的越少，右轮的pwm应该越少。方向偏的多少（偏差error）就是[yaw的目标值]-[yaw的当前值]。
出现的问题原因：左轮输入的Input和右轮驱动输入的Input是一样的，但是由于左轮和右轮的执行器有差异，导致左轮的Output1和右轮的Output2的值不一样。于是我们保持左轮依然使用开环系统，而把右轮系统改成可以调整转速的闭环系统。这样使得输入右轮系统的数值就不仅仅是Input了，还增加了电子陀螺仪反馈回来的偏转角（jaw）的误差值error。把这两个数值通过控制器处理后，再输入到执行器，让右轮最终产生的Output2等于左轮的Output1。
（3）在系统的Controller部分引用PID比例部分
pwm=error*Kp+input
error=yaw的目标值-yaw的当前值
根据error的值的大小设定一个比例系数Kp，Kp是一个系数常数，需要根据系统的性能手动设定。然后加上原来input，形成新的pwm值。
因为电子陀螺仪的采样率大概为100ms一次。所以需要在计算机里每100ms 采样一次得到yaw的值：error = [yaw的目标值]-[yaw的当前值]。新的pwm=error*Kp+input，用来驱动右轮的电机。
Kp值的选取：只有在Kp选取到一个合适的值，小车才能保持直线。
稳态误差产生的原因：纠正的过程中，当前的测量值会跟目标值越来越小，那么error就会变得很小，使得error*Kp 也会变得很小； error*Kp 由于过小就会失去纠正的作用，离目标值始终存在一个误差，称之为稳态误差。
（4）在系统的Controller部分引用PID积分部分
问题：合适的Kp是很难找的，我们可以引入E*Ki（误差的积分）。当 error*Kp 部分的量纠正不够的时候，可以靠 I 部分的量（对每100ms测量出来的误差进行累加）来补充上去。
假设：a. 开始向右偏转，只要不向左偏转，I 值是一直增加的。b. P 的输出一开始是比较猛的，但是当越来越靠近目标。c. 当 P 因误差 error 过小失去调整作用时，可以用 I 积分部分来弥补比例部分的稳态误差。
pwm = error * Kp + E * Ki + input，
E = 每次测量的error 的累加和，系统每100ms测量一次jaw的值，每次测量的jaw的值和jaw的目标值有一个差值，我称之为err,把每次测量的err累加在一起就是E。
（5）在系统的Controller部分引用PID微分部分
我们可以通过限制 I 的最大值的方法，来进行限制。也可以再增加一个微分量 D*Kd，Kd为系数常数。每100ms测量一次jaw偏转角，而D指的就是这一次的误差和上一次测量的误差之间的差值，是一个反映误差纠正快慢的量，重点是负值。
在图上看，就是图像变化陡峭的部分。当Input加上这个值以后，系统可以有效抑制运行过程中变化过快的部分，比如振荡期。而对变化缓慢的部分，比如稳定期，微分部分几乎就不起作用。有点像起一个柔和的缓冲作用。
pwm = error * Kp + E * Ki + D * Kd + input,
D=当前一次测量的error - 上一次测量的error，每100ms测量一次jaw，并计算出err，D反应的是系统的err变化率。所以err变化剧烈的区域，D会对总的输出值进行抑制，防止剧烈的振荡，而稳定之后的区域，D对输出值的影响就很小了。
三、程序编写
1. 核心公式
pwm = error * Kp + E * Ki + D * Kd + input
2. 相关参数及说明
最后每100ms 计算一次公式 pwm = error * Kp + E*Ki + D * Kd + input ，把计算出来的pwm值作为右轮的电机驱动的参数，代入电机的驱动程序里，根据实际场景调整一下系统参数Kp，Ki。