电机控制算法的作用是接受指令速度值，通过运算向电机提供适当的驱动电压，尽快地和尽快平稳地使电机转速达到指令速度值，并维持这个速度值。换言之，一旦电机转速达到了指令速度值，即使在各种不利因素(如斜坡、碰撞之类等使电机转速发生变化的因素)的干扰下也应该保持速度值不变。为了提高机器人小车控制系统的控制精度，选用合适的控制算法显得十分必要。
控制算法是任何闭环系统控制方案的核心，然而并非越复杂、精度越高的算法越好，因为比赛要求非常高的实时性，机器人必须在非常短的时间内作出灵敏的反应，所以现代的一些先进控制算法，比如模糊控制、神经元网络控制等就不能应用到小车控制系统里。本系统选用了最常规、最经典的PID控制算法，通过实际应用取得了很好的效果。
1 比例项
控制回路中的第一个偏差转换环节就是比例项。这一环节简单地将偏差信号乘以常数K 得到新的CV值(值域为-100~100)。基本的比例控制算法如下：
loop:
PV=ReadMotorSpeed()
Error=SP-PV
CV=Error*Kprop
Setpwm(cv)
Goto loop
上一段程序中的SetPWM()函数并非将CV值作为绝对的PWM占空比来对待。否则，不断降低的偏差值会使输出值接近零，而且由于电机工作时需要持续的PWM信号，控制系统将会使电机稳定在低速运转状态上，从而导致控制系统策略失败。
相反，CV值一般被取作当前PWM占空比的改变量，并被附加到当前的PWM占空比上。这也要求SetPWM()函数必须将相加后得到的PWM占空比限制在0%~100%。正的CV值将使电机两端电压增加。负的CV值将使电机两端电压降低。如果CV值等于0，则无需改变但前占空比。较低的K 值会使电机的速度响应缓慢，但是却很平稳。较高的K 值会使速度响应更快，但是却可能导致超调，即达到稳定输出前在期望值附近振荡。过高的K 值会导致系统的不稳定，即输出不断震荡且不会趋于期望值。
2 微分项
任何变量的微分项被用来描述该变量是如何相对于另一个变量(多位时间)变化的。换句话说，任何变量的微分项就是它随时间的变化率。如位移随时间的变化率是速度。速度相对于时间的微分是加速度。
在PID控制器中，值得关心的是偏差信号相对于时间的微分，或称变化率。绝大多数控制器将微分项定义为：
Rate=(E-E )/T
式中，E为当前偏差，E 为前次偏差值，T为两次测量的时间间隔。负的变化率表明偏差信号的改善。当微分项被具体应用于控制器中时，将一个常数乘以该微分项，并将它加到比例项上，就可以得到最终的CV值计算公式：
CV=( K E)+( K Rate)
当偏差信号接近零时，CV值将为负，所以当偏差信号开始改善时，微分项的作用将逐渐减弱校正输出量。在某些场合下，微分项还有利于超调量的消除，并可以允许使用较大的K 值，从而可以改善响应的快速性。微分环节还预示了偏差信号的变化趋势。当控制对象对控制器的输出响应迟缓时，微分环节的作用尤为明显。
含有微分项的控制算法的伪代码实现如下：
loop:
PV=ReadMotorSpeed()
LastError=Error
Error=SP-PV
Rate=Error-LastError
CV=Error*Kprop+Krate*Rate
SetPWM(CV)
Goto loop
3 积分项
积分正好与微分相对。假如有一个描述变化率(微分)的表达式，那么对该表达式的积分就将得到随时间变化的原物理量。如加速度的积分是速度，速度的积分是位移。
在PID控制回路中，偏差的积分代表从控制开始时算起所有偏差积累的总和。该总和被常数K 所乘后再添加到回路输出中。在回路中，如果没有积分环节，尽管控制系统也会趋于稳定，但是由于某种原因输出值可能最终也无法达到SP值。
一个简单但完全的PID控制器地伪代码实现如下：
loop:
PV=ReadMotorSpeed()
LastError=Error
Isum=Isum+Error
Error=SP-PV
Rate=Error-LastError
CV=Error*Kprop+Krate*Rate+Kint*Isum
SetPWM(CV)
Goto loop
由于积分项会越来越大，这就会使控制回路在SP值的改变时响应变慢，某些应用场合在CV值达到取值边界(如为：-100~100)时会停止累加Isum。在SP值改变时，也可以除去Isum项。
通过测试，发现计数器0初值范围在 1~6230 以内，可控硅能正常工作，且能够实现调节电机从不能起动到全速运行的全过程。
程序中定时器0工作在方式 1，用 To表示定时器0计数初值的十进制值，定时器 0置入的初值计算方法为：
TH 0=(65536-To )/ 256
TL 0=(65536-To) % 256
这样，通过外部函数改变To的值就可以调节电机的转速了。
1.P1控制算法
系统中控制器采用P1控制算法，其离散增量算式为：
△u(k)=Kp△e(k)+Kie(k)，式中，△u(k)、△e(k)、e(k)分别为第k步控制信号增量、偏差信号增量和偏差信号；kp、Ki分别为比例增益和积分增益。
为克服积分饱和，本文采用积分分离P1控制算法，当误差信号小于给定误差时计算积分控制量。由于被控电机通过To起作用,同时，To的变化范围设定为1~6230，而速度的变化范围为O~60，因此比例系数为：Kp=6230/60=103.8，本文取Kp=100，积分增益K1的值通常取得很小。
2.同速过程分析
将目标电机和被控电机的测速中断计数分别用N1、N2表示，则目标电机和被控电机的转速分别为：Vm=6000/N1,Vs=6000/N2，式中，Vm，Vs分别为目标电机和被控电机的转速。
如果Vm>Vs，且To-100(Vm-Vs)>1，那么To-=100(Vm-Vs)；如果Vm 同时须注意到山值的调节依赖于两个电机都能正常转动的情况，即N1和N2都不为零；目标电机是无需担心，因为它完全受人为控制，一般是不会将其速度调节至零；对于被控电机而言，其初始速度很可能为零，或者为其它某一接近零的值，这就意味着可能需要很长一段时间，才能检测到它旋转一圈，针对此情况，可在软件中设置强行转动，即当To的值在6230附近时，强行将To值变小，从而提前开始同速的时间。 双电机同速时特点为：To值对应于目标电机速度，因此开始的时候应尽量使To的值向最终值贴近，这里可使用一个巧妙算法，目标电机速度对额定速度百分比PERC=Vm/60=100/N1，则被控电机To值大致为To=6230(1-PERC) 由于控制被控电机的电流含有谐波分量信号，因此所计算的To值与最终To值会有一些偏差(一般偏小)，但是通过这样计算之后，电机速度同步过程时间会缩短很多。 3.同相措施 本设计要求两个电机不但同速，而且同相，同速是同相的基础，实现同速之后方能实现同相。当两个电机达到同速后，所产生相位差是恒定的，对N1计数完毕到N2开始计数这段时间进行计时，假设计数值为N3,要使两个电机同相，可以简单理解为使N3值变为零。若不考虑相差一个360度相位角，增大或减小被控电机速度都可以达到同相效果，我们选择速度在30转/分以上用减小速度来达到同相，反之则选择增加速度来达到同相。具体措施是： (1)速度大于30转/分的区间，将被控电机速度设为零，即To=6230，等待时间计数(N1-N3)后，再将速度值恢复为同速时的值，两个电机即实现同相。 (2)速度小于30转/分的区间，将被控电机速度设为最大值，即To=1，等待时间Vm×N3/(60-Vm)后，再将速度值恢复为同速时的值，两个电机即达到同相。 但是以上方法限于理想状况，事实上，由于惯性作用，电机速度不可能会有瞬间变化，也就并不能这么简单的达到同相，对于同相控制，积分起到关键作用。利用比例积分思想的同相算法如下： (3)速度大于30转/分，减小被控电机的速度，并且To变化值与(N1-N3)/N1成比例，同时与(6230-To)成比例，因此To+=(6230-To)×(N1-N3)/N1。 (3)速度小于30转/分，增大被控电机速度，To的变化值与N3/N1成比例，同时与To成比例，所以To-=To×N3/N1。 以上算法每执行一次，需将To恢复为同速时的数值，即恢复电机同速转动，如此调节电机的速度与相位，直到N3变为零。 小结 对电机随动系统实现了一种基于单片机的控制，它克服了传统模拟控制电路易受电网参数波动以及原件参数分散性的影响，实验表明，这种控制方法在低速随动控制系统中能够保证满意性能。 本文来源：百度文库