2.3.1 解决的问题
2 a) ]5 r; M' E9 _4 `
! P- g* L& U4 W0 R& \0 ?

解决带编码器直流电机的速度闭环问题。

5 J" b# m7 m/ Y1 o& ?5 k

2.3.2 PID理论
, A3 J  G, `0 W+ n0 T

将偏差的比例、积分、微分，通过线性组合构成控制量，用控制量对被控对象进行控制，这样的控制器称为PID控制器。在连续空间中，我们通常探讨模拟PID的控制原理，如图所示：

我们这里用电机速度控制为例，讲解PID控制系统。r(t)为设定电机速度、y(t)为实际电机速度、e(t)=y(t)-r(t)为速度差值作为PID控制器的输入、u(t)为PID控制器的输出，作用到被控对象电机上。根据模拟PID控制器，科学家们也得出了模拟PID控制的公式，如图所示：

7 H9 Q. T# C0 G6 v2 S

其中Kp、Ti、Td，分别为控制器的比例系数、积分系数、微分系数。该理论用在控制的例子比比皆是。但是模拟PID控制系统是在连续空间的上描述的，无法在计算机上用代码实现。于是就有数字PID控制理论，将连续空间的PID控制系统在离散空间上描述。积分变成了求和、微分变成了求斜率，于是就出现数字PID控制系统的理论公式，如图所示：

8 V0 G1 B- X) d! k4 i

* q+ c0 u$ G( G6 v9 m8 f: E( \

其中Kp、Ti、Td和上面描述的一样，T为采用周期，ek是本次差值，ek-1上一次的差值，直接通过模拟PID转化的数字PID又叫做位置式PID，该方式的PID的输出直接是控制量，非常不适合经常出现异常的系统，另外一种方式是增量式PID，每次只输出一个正向或者反向的调节量，就算出现异常，也不会产生巨大的影响。具体数学公式如下所示：该方法较多的应用于生产生活中，本论文中电机的速度PID控制当然也不例外。

/ g. u6 ^2 I- L) }3 b0 u; ~% U5 E' k

有了上面的理论基础，开始代码实现的介绍。首先就是明确增量式PID系统的输入、输出、控制对象。将速度的设定值和速度的测得值作为PID控制器的输入参数，PID的输出参数为对PWM的调节偏差，控制对象PWM进而驱动电机达到设定速度。以上内容确定之后，就是PID控制器的代码部分了。其实仔细看看增量式PID就只有一个公式，所以使用代码实现并不困难。如下所示核心代码就这一句。

$ q6 e" W4 i8 q4 y) ~* z4 z; k$ }3 |

完成上面的代码，只是完成速度PID的一部分，剩下的是尤为重要的PID参数整定。该整定方法丰富多样，最为准确的是模型计算，但是对于我们做机器人多使用试凑法。虽然需要调节一段时间，但是不需要对机器人进行建模。试凑法一般按照P、I、D的顺序进行调节。

  h! W7 q! b* j

初始时刻将Ki和Kd都设置成0，按照经验设置Kp的初始值，就这样将系统投入运行，由小到大调节Kp。求得满意的曲线之后，若要引入积分作用，将Kp设置成之前的5/6,然后Ki由小到大开始调节。达到满意效果之后，若要引入微分作用，将Kd按照经验调节即可。经过有规律的试凑，最终达到一个我们满意的就行。

+ [& r5 G$ u. ~* R* d! f

2.3.3 代码分享

(1）pid.h

1. #include "pid.h"
   
2. 
   $ x: a( Z1 B2 t& j) A! v7 q
3. 
   
4. struct pid_uint pid_Task_Letf;
   5 W( M% E7 B8 u! h3 G( a
5. struct pid_uint pid_Task_Right;
   
6. 
   : W" X; I3 O' O+ ?# F
7. /****************************************************************************
   & f% q. B7 u1 d, F; p
8. *函数名称：PID_Init(void)
   
9. *函数功能：初始化PID结构体参数
   7 U/ z. u$ c7 O1 i# I
10. ****************************************************************************/
    
11. 
      u: j4 `7 c. F! J; ?
12. void PID_Init(void)
    * ?$ [) e+ l& u2 l' q, b
13. {
    8 a. n, \6 ]. A5 Y* |
14. //乘以1024原因避免出现浮点数运算，全部是整数运算，这样PID控制器运算速度会更快
    $ H& [, j$ y& Z1 D7 \
15. /***********************左轮速度pid****************************/
    4 e9 M/ B& i  ~( u+ V
16.         pid_Task_Letf.Kp = 1024 * 0.5;//0.4
    # N5 _, D( g; e! `# U3 _
17.          pid_Task_Letf.Ki = 1024 * 0;        
    
18.         pid_Task_Letf.Kd = 1024 * 0.08;
    : H( V+ v9 |. f* x( k0 v0 s
19.         pid_Task_Letf.Ur = 1024 * 4000;
    
20.         pid_Task_Letf.Adjust   = 0;
    - `" W! H6 ]1 [4 u, D
21.         pid_Task_Letf.En       = 1;
    
22.         pid_Task_Letf.speedSet = 0;
    
23.         pid_Task_Letf.speedNow = 0;
    
24.         reset_Uk(&pid_Task_Letf);               
    4 R4 y/ Z! q) e
25. /***********************右轮速度pid****************************/
    
26.         pid_Task_Right.Kp = 1024 * 0.35;//0.2
    
27.          pid_Task_Right.Ki = 1024 * 0;        //不使用积分
    
28.         pid_Task_Right.Kd = 1024 * 0.06;
    0 V2 M  d% D8 e
29.         pid_Task_Right.Ur = 1024 * 4000;
    
30.         pid_Task_Right.Adjust   = 0;
    
31.         pid_Task_Right.En       = 1;
    
32.         pid_Task_Right.speedSet = 0;
    . b4 V7 K% l* g3 O; O7 y! M2 _
33.         pid_Task_Right.speedNow = 0;
    
34.         reset_Uk(&pid_Task_Right);
    
35. }
    - p- \& E: y, z0 c0 N
36. 
    2 q. m# i8 I; A- l
37. /***********************************************************************************************
    
38. 函 数 名：void reset_Uk(PID_Uint *p)
    * ~; o. [! d: b" c( r) ?
39. 功    能：初始化U_kk,ekk,ekkk
    
40. 说    明：在初始化时调用，改变PID参数时有可能需要调用
    
41. 入口参数：PID单元的参数结构体 地址
    % U8 Y2 U( w5 b" u- S
42. ************************************************************************************************/
    
43. 
    8 W$ ^/ R/ k# H) }# M$ M- V* p
44. void reset_Uk(struct pid_uint *p)
    
45. {
    - `5 \, N* |! `$ |# Z4 Y+ h
46.         p->U_kk=0;
    
47.         p->ekk=0;
    
48.         p->ekkk=0;
    7 M8 j5 ^2 v0 W5 n$ d$ U
49. }
    
50. 
    
51. /***********************************************************************************************
    
52. 函 数 名：s32 PID_commen(int set,int jiance,PID_Uint *p)
    
53. 功    能：PID计算函数
    
54. 说    明：求任意单个PID的控制量
    
55. 入口参数：期望值，实测值，PID单元结构体
    
56. 返 回 值：PID控制量
    
57. ************************************************************************************************/
    
58. 
    8 X. Y# D% x. k) N/ `
59. s32 PID_common(int set,int jiance,struct pid_uint *p)
    
60. {
    0 Z- M4 K& s. ?1 @1 g
61.         int ek=0,U_k=0;
    + M0 s) ]. {/ ~. D5 q
62. 
    / W% b: F% U+ o( ?$ I
63.         ek=jiance - set;                                                               
    
64.         
    
65.         U_k=p->U_kk + p->Kp*(ek - p->ekk) + p->Ki*ek + p->Kd*(ek - 2*p->ekk + p->ekkk);
    " J, x0 @0 x, ^  _; H. }& e  y, Z
66.         
    2 a% W: e7 x8 \6 G9 F5 p) p
67.         p->U_kk=U_k;
    
68.     p->ekkk=p->ekk;
    
69.         p->ekk=ek;
    - [3 M9 O, ]% d/ O& s! V
70.         
    
71.         if(U_k>(p->Ur))                                                   
    9 D4 a8 u/ Q( v8 g0 B' r: j$ `/ D
72.                 U_k=p->Ur;
    
73.         if(U_k<-(p->Ur))
    4 x# [& H$ }* _4 `/ M
74.                 U_k=-(p->Ur);
    $ }% Y" Z: N: ]6 w
75.         
    
76.         return U_k>>10;
    * }& A3 o/ W4 g; l# ^( d
77. }
    8 {+ l8 z" m7 _% H2 d; R
78. 
    
79. /***********************************************************************************
    % P- a, |3 b& N, I& I+ _3 }
80. ** 函数名称 ：void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
    ( l& e0 o9 g1 o$ p, ^2 n6 ~: {2 u! u
81. ** 函数功能 ：pid选择函数              
      o' I) i9 r* e7 h( G; q( n
82. ***********************************************************************************/
    
83. 
    
84. void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
    / s* W6 p0 C, y4 ?+ g5 x
85. {
    * S5 k4 \- ^  B% ~, R' ]8 V
86.         /**********************左轮速度pid*************************/
    3 o7 L! a# U; f! O; |
87.         if(pl->En == 1)
    ; A. r- [8 P1 g+ ^5 n" u
88.         {                                                                        
    . @7 k, G3 m4 V8 r! J
89.                 pl->Adjust = -PID_common(pl->speedSet, pl->speedNow, pl);               
    
90.         }        
    
91.         else
      w" l+ U) W& u* }, M
92.         {
    
93.                 pl->Adjust = 0;
    / R. ?; E5 P4 l$ w' F
94.                 reset_Uk(pl);
    
95.                 pl->En = 2;
    0 [& d6 S8 s6 R2 a3 `
96.         }
    
97.         /***********************右轮速度pid*************************/
    
98.         if(pr->En == 1)
    
99.         {
    
100.                 pr->Adjust = -PID_common(pr->speedSet, pr->speedNow, pr);               
     
101.         }        
     $ B5 Y' O  e9 b* o: `
102.         else
     
103.         {
     
104.                 pr->Adjust = 0;
     
105.                 reset_Uk(pr);
     . Q% v/ r% a, @
106.                 pr->En = 2;
     9 _# q$ W% q9 X
107.         }
     
108. }
     , d3 l. D% s6 K% {
109. 
     
110. /*******************************************************************************
     % v* J5 i3 z2 R- [
111. * 函数名：Pid_Ctrl(int *leftMotor,int  *rightMotor)
     
112. * 描述  ：Pid控制
     . Y1 w7 b6 `: K2 A$ o3 A# o% i8 E
113. *******************************************************************************/
     " n6 l5 \9 T+ q& a% Y) {* a  E! `8 @
114. 
     
115. void Pid_Ctrl(int *leftMotor,int  *rightMotor)
     
116. {
     
117.         Pid_Which(&pid_Task_Letf, &pid_Task_Right);
     0 x  Q5 e' S" g" |- z! q+ r
118.         *leftMotor  += pid_Task_Letf.Adjust;
     
119.         *rightMotor += pid_Task_Right.Adjust;
     : v6 ~4 a: u1 b; v& `! w7 Y! m
120. }
     

复制代码

(2)main.c

3 _* u- M5 O' i  j

5 y9 x6 c, \& m" }# M" X

1. #include "sys.h"
   
2. 
   
3. //====================自己加入的头文件===============================
   - R8 M# Q8 P/ X/ O# H4 R" F
4. #include "delay.h"
   / K0 M# {8 S5 S5 @4 l1 O- f, Y
5. #include "led.h"
   
6. #include "encoder.h"
   
7. #include "usart3.h"
   
8. #include "timer.h"
   * ]; b3 I* [, Q& K0 }' H6 j1 k1 M: b( S
9. #include "pwm.h"
   3 g9 _$ @5 Y9 r( X  P# W1 j; W
10. #include "pid.h"
    ; q6 ?% B3 ?3 c; H- g3 c' V3 |
11. #include "motor.h"
    
12. #include <stdio.h>
    ( {( A, \" Q% ^% a# @4 J5 z- F
13. /*===================================================================
    
14. 程序功能：直流减速电机的速度闭环控制测试
    & q4 I7 K- J) N+ O2 G
15. 程序编写：公众号：小白学移动机器人
    $ j5 O4 _+ `. s& q6 S4 S
16. 其他    ：如果对代码有任何疑问，可以私信小编，一定会回复的。
    ( r) C. d. M2 p& B. y+ m! P
17. =====================================================================
    
18. ------------------关注公众号，获得更多有趣的分享---------------------
    5 d4 b' Y; l: W2 o( B4 z* F
19. ===================================================================*/
    ( p1 {$ p" a" l3 Z
20. int leftSpeedNow  =0;
    
21. int rightSpeedNow =0;
    
22. 
    
23. int leftSpeeSet   = -300;//mm/s
    
24. int rightSpeedSet = -300;//mm/s
    
25. 
    " @9 b' {/ R7 p9 f* @4 u
26. int main(void)
      S0 L7 ?" i0 P, N7 o
27. {
    & w8 H9 U$ \+ j
28. 
    1 S+ R) \. c4 H3 {$ V0 D
29.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
    8 g- c+ j2 E# v) A' a8 t4 A* P
30.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
    8 b% F, Q7 i+ C4 {
31.         
    4 ~1 A; [+ p. f
32.         MY_NVIC_PriorityGroupConfig(2);        //=====设置中断分组
    
33.         
    
34.         delay_init();                            //=====延时函数初始化
    
35.         LED_Init();                     //=====LED初始化    程序灯        
    
36.         
    
37.         usart3_init(9600);              //=====串口3初始化  蓝牙 发送调试信息
    
38. 
    ' w& |  {- T: b. ^8 Y' m
39.         Encoder_Init_TIM2();            //=====初始化编码器1接口
    
40.         Encoder_Init_TIM4();            //=====初始化编码器2接口
    : k  Y4 ]2 m3 b, V
41.         
    2 v* Q( o3 [5 _( e4 \
42.         Motor_Init(7199,0);             //=====初始化PWM 10KHZ，用于驱动电机 如需初始化驱动器接口
    
43.         
    
44.         TIM3_Int_Init(50-1,7200-1);     //=====定时器初始化 5ms一次中断
    . H$ E1 e* t. l
45. 
    + I+ R4 \5 B/ i, u) t* x  D# i
46.         PID_Init();                                                //=====PID参数初始化
    
47.         
    - }% \% |9 S1 V9 S5 m) c
48.         while(1)
    , n  t) w0 [5 ?* S$ t  M6 [
49.         {
    5 Z/ G4 w1 v+ q0 s
50.                 //给速度设定值和实时值赋值
    ; M2 G- x  e( F' W7 I& H" ~( y4 o  z
51.                 pid_Task_Letf.speedSet  = leftSpeeSet;
    , D. t0 d6 W( t2 G
52.                 pid_Task_Right.speedSet = rightSpeedSet;
    
53.                 pid_Task_Letf.speedNow  = leftSpeedNow;
      \. S2 [. Z( ]. {+ d" t4 U
54.                 pid_Task_Right.speedNow = rightSpeedNow;
    ! S: `/ S* i) \
55.                
    0 f# H2 |1 `- d7 n
56.                 //执行PID控制函数
    
57.                 Pid_Ctrl(&motorLeft,&motorRight);
    " P, ~4 D6 ^" J: c+ H7 T
58.                
    . e9 [1 j& ]% m$ u: j
59.                 //根据PID计算的PWM数据进行设置PWM
      P- M. @' g) T& J# H
60.                 Set_Pwm(motorLeft,motorRight);
    
61.                
    
62.                 //打印速度
    
63.                 printf("%d,%d\r\n",leftSpeedNow,rightSpeedNow);
    . k! x# L& r) g- g
64.         }
    - q9 Y  y, Y- C
65. }
    
66. 
    # M5 |* c0 h6 a3 J2 V/ f
67. //5ms 定时器中断服务函数
    
68. 
    
69. void TIM3_IRQHandler(void)                            //TIM3中断
    
70. {
    
71.         if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否
    7 S0 H/ p" Y+ k* `3 V$ a
72.         {
    9 S( x+ X( ?) K: g9 v* g
73.                 TIM_ClearITPendingBit(TIM3, TIM_IT_Update);   //清除TIMx的中断待处理位
    2 y8 n; v  e0 O! D
74.                
    
75.                 Get_Motor_Speed(&leftSpeedNow,&rightSpeedNow);//计算电机速度
    
76.                
    
77.                 Led_Flash(100);                               //程序闪烁灯
    1 Z  t- o9 B3 t6 e- G1 G
78.         }
    
79. }

复制代码

; {  R; f  V) {0 j' [& X3 q: [) f/ K
2.3.4 总结

8 O% H5 @: r1 n

以上三篇内容，是关于直流减速电机的PWM控制、速度测量以及最后电机速度的闭环控制。现在对于电机的简单控制基本告一段落，对于做一个ROS小车的电机控制，这里基本是足够的。下面我们会介绍使用IIC+DMP获取MPU6050数据。

: b- k: |6 Y% i' h: ^( s% ~
  r& Y1 m8 k, N5 p1 r5 s6 ?

$ \8 A1 n, @( P3 f& Q