2.3.1 解决的问题

( C/ u% C0 f5 y
+ ^# O8 K( n5 G

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

2.3.2 PID理论
" G  l: T+ V. j8 }
, Y. R: b2 ~7 K. z7 m

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

6 Z( U. F" ]& ?& @# b$ n

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

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

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

- k' L0 D- Y% M" H

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

# h, m1 l% M- y4 q6 }7 e& J: L$ b

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

  u* r+ r* I* ~- r# f4 r5 Z. l( V7 x. M

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

; `- H! @; I& U8 @: ~5 |& X  D2 b

2.3.3 代码分享

6 A# v! t4 ^* _* R3 f+ T

(1）pid.h

1. #include "pid.h"
   , k. i& e/ M  Q1 E3 i" T8 i
2. 
   
3. 
   
4. struct pid_uint pid_Task_Letf;
   % w/ O* H) |2 u$ _
5. struct pid_uint pid_Task_Right;
   & ^/ n1 ^5 ?! {
6. 
   
7. /****************************************************************************
   ) P; U  R. |% Q5 e7 y) \4 W: g
8. *函数名称：PID_Init(void)
   
9. *函数功能：初始化PID结构体参数
   2 d' T! v, P, F
10. ****************************************************************************/
    1 L6 L' ~& [3 A1 T
11. 
    ! b$ W) D9 e4 z7 K! B, X+ O4 ]
12. void PID_Init(void)
    9 r6 X* L1 }4 d' x6 j; W
13. {
    # L% p, i- h" A) [  ]) Y! Z: A  q' S
14. //乘以1024原因避免出现浮点数运算，全部是整数运算，这样PID控制器运算速度会更快
    
15. /***********************左轮速度pid****************************/
    3 E! U4 _0 z! y1 g
16.         pid_Task_Letf.Kp = 1024 * 0.5;//0.4
    * \! c' ?& O2 W  |% D" I
17.          pid_Task_Letf.Ki = 1024 * 0;        
    , f) Y" U$ I8 {; X: C
18.         pid_Task_Letf.Kd = 1024 * 0.08;
    
19.         pid_Task_Letf.Ur = 1024 * 4000;
    
20.         pid_Task_Letf.Adjust   = 0;
    
21.         pid_Task_Letf.En       = 1;
    2 N) X, a1 n* h9 n' Y2 v3 G8 F2 j. C
22.         pid_Task_Letf.speedSet = 0;
    
23.         pid_Task_Letf.speedNow = 0;
    
24.         reset_Uk(&pid_Task_Letf);               
    
25. /***********************右轮速度pid****************************/
    
26.         pid_Task_Right.Kp = 1024 * 0.35;//0.2
    % Z* H  ]. b9 T: C% P
27.          pid_Task_Right.Ki = 1024 * 0;        //不使用积分
    ! }0 t4 v% @3 i& _. ?& ?) L
28.         pid_Task_Right.Kd = 1024 * 0.06;
      Q3 ^1 K: C) T, a# }
29.         pid_Task_Right.Ur = 1024 * 4000;
    
30.         pid_Task_Right.Adjust   = 0;
    " `& K0 q* b3 M% Q* {/ A$ [+ T7 E
31.         pid_Task_Right.En       = 1;
    3 |5 R# P# Q9 ?, v0 X/ o! o
32.         pid_Task_Right.speedSet = 0;
    7 d6 e7 w5 R, x& W
33.         pid_Task_Right.speedNow = 0;
    
34.         reset_Uk(&pid_Task_Right);
    6 `, L% C; H: u* O/ r
35. }
    ) Z+ h1 V8 a; Q+ P$ h/ r
36. 
    ( U$ L# [, o3 X' {9 }
37. /***********************************************************************************************
    
38. 函 数 名：void reset_Uk(PID_Uint *p)
    ( [7 B. v/ ^: q" U
39. 功    能：初始化U_kk,ekk,ekkk
    
40. 说    明：在初始化时调用，改变PID参数时有可能需要调用
    4 [: s0 B* j# C% D9 A" c4 d
41. 入口参数：PID单元的参数结构体 地址
    5 B2 V3 R0 l( |$ A2 q1 U
42. ************************************************************************************************/
    
43. 
    
44. void reset_Uk(struct pid_uint *p)
    
45. {
    
46.         p->U_kk=0;
    ) y. q* E+ M0 r2 u
47.         p->ekk=0;
    
48.         p->ekkk=0;
    & I6 j* ~& @7 |
49. }
    ) y0 L. Y$ E0 a5 p$ o8 G' J- g1 C
50. 
    
51. /***********************************************************************************************
    
52. 函 数 名：s32 PID_commen(int set,int jiance,PID_Uint *p)
    
53. 功    能：PID计算函数
    
54. 说    明：求任意单个PID的控制量
      [# r! Q. z- G' j" c% N1 |' o" p- q
55. 入口参数：期望值，实测值，PID单元结构体
    " u2 J. _, D1 Z1 W. ]/ j" Q( r
56. 返 回 值：PID控制量
    
57. ************************************************************************************************/
    ! ?# \% Q& }" U2 g( y/ E
58. 
    
59. s32 PID_common(int set,int jiance,struct pid_uint *p)
    $ r  U4 I3 u+ j1 Z
60. {
    ) X, o9 g& z+ y7 l# j& [
61.         int ek=0,U_k=0;
    
62. 
    
63.         ek=jiance - set;                                                               
    7 x" a5 c" y# {5 p8 m
64.         
    ( `4 f7 t9 S, D: \: C
65.         U_k=p->U_kk + p->Kp*(ek - p->ekk) + p->Ki*ek + p->Kd*(ek - 2*p->ekk + p->ekkk);
    
66.         
    + R( K7 c2 v- m7 e. ^
67.         p->U_kk=U_k;
    
68.     p->ekkk=p->ekk;
    2 \' Y4 ^5 j% i- D2 l) t# u
69.         p->ekk=ek;
    
70.         
    
71.         if(U_k>(p->Ur))                                                   
      Z# q+ R. H7 L6 W
72.                 U_k=p->Ur;
    # z. j1 z* Q9 \7 t8 o& n& _' `
73.         if(U_k<-(p->Ur))
    ! t3 K1 u- r6 e9 m) }
74.                 U_k=-(p->Ur);
    
75.         
    
76.         return U_k>>10;
      x' h! E+ I" i) x
77. }
    
78. 
    
79. /***********************************************************************************
    9 K* H1 H+ R! O( Y4 R/ W
80. ** 函数名称 ：void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
    - }/ @; m6 d' W0 z1 W) ?# |& o
81. ** 函数功能 ：pid选择函数              
    & E! [( W  \3 T4 ?# p9 Y. M1 [' e- T
82. ***********************************************************************************/
    % j2 O$ [+ g2 a; U; ?
83. 
    
84. void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
    
85. {
    
86.         /**********************左轮速度pid*************************/
    
87.         if(pl->En == 1)
    6 u) N% k1 H8 x5 x
88.         {                                                                        
    
89.                 pl->Adjust = -PID_common(pl->speedSet, pl->speedNow, pl);               
    3 o  s6 a- a$ q" x' ]9 Q3 p% {
90.         }        
    ! \6 J0 l5 T% T1 m; z0 Z1 Z
91.         else
    8 j! l7 b1 g, L; l. s+ U
92.         {
    
93.                 pl->Adjust = 0;
    4 h( s6 H6 ~, ^1 f8 [( y
94.                 reset_Uk(pl);
    
95.                 pl->En = 2;
      y( D3 k% j0 K1 ?" Y
96.         }
    ( K, ^, K8 m8 e
97.         /***********************右轮速度pid*************************/
    8 Y' Q+ N4 O# W% N# H. _
98.         if(pr->En == 1)
    5 u6 R; e  D- Q
99.         {
    8 W; c# x* o( [4 I
100.                 pr->Adjust = -PID_common(pr->speedSet, pr->speedNow, pr);               
     % Y' G/ _% o* I7 p9 z$ i' t
101.         }        
     
102.         else
     6 v! j  o" `- S/ P
103.         {
     
104.                 pr->Adjust = 0;
     * G5 M. T8 L/ D5 z' h' p5 a1 ?
105.                 reset_Uk(pr);
       K& j- u& W3 f$ d% Q
106.                 pr->En = 2;
     / o* |: Q3 X1 \- b2 d
107.         }
     
108. }
     
109. 
     
110. /*******************************************************************************
     
111. * 函数名：Pid_Ctrl(int *leftMotor,int  *rightMotor)
     
112. * 描述  ：Pid控制
     ( a0 v1 r4 w8 i6 s/ J$ N( Z9 X
113. *******************************************************************************/
     
114. 
     
115. void Pid_Ctrl(int *leftMotor,int  *rightMotor)
     
116. {
     
117.         Pid_Which(&pid_Task_Letf, &pid_Task_Right);
     
118.         *leftMotor  += pid_Task_Letf.Adjust;
     1 y5 x: t( O) d* j
119.         *rightMotor += pid_Task_Right.Adjust;
     
120. }
     + ?+ |3 d$ t* B

复制代码

(2)main.c

0 d  q6 j  y& O3 w- d6 p( N

1. #include "sys.h"
   
2. 
   
3. //====================自己加入的头文件===============================
   ) @" m4 V1 @, z. l: t& k* T8 [
4. #include "delay.h"
   5 b. `1 c5 ~5 O5 y+ L. Z. ?) ?
5. #include "led.h"
   ' C/ i$ S% A- d; E4 q1 |. Z
6. #include "encoder.h"
   3 Z8 F) m' G3 r4 P0 w9 i9 X; x+ h
7. #include "usart3.h"
   
8. #include "timer.h"
   & Q3 e6 ?4 w$ u# v6 H3 K: W0 ^
9. #include "pwm.h"
   
10. #include "pid.h"
    6 d% J6 L, I  ~) p/ ?
11. #include "motor.h"
    
12. #include <stdio.h>
    + N1 {- p5 D; t7 C+ l( J
13. /*===================================================================
    % ~8 ~$ s/ B+ L& N1 @8 |
14. 程序功能：直流减速电机的速度闭环控制测试
    
15. 程序编写：公众号：小白学移动机器人
    8 ~& t/ M6 S6 L2 L
16. 其他    ：如果对代码有任何疑问，可以私信小编，一定会回复的。
    
17. =====================================================================
    7 d; `- l% r! W* [4 P7 ~
18. ------------------关注公众号，获得更多有趣的分享---------------------
    4 K' Q1 u( \9 A% g0 J% W& P
19. ===================================================================*/
    4 V3 n; v8 n) d* C. z! L9 W6 Y
20. int leftSpeedNow  =0;
    
21. int rightSpeedNow =0;
    
22. 
    7 \5 g2 B5 |0 H) j
23. int leftSpeeSet   = -300;//mm/s
    " V  C) u  A: J3 }
24. int rightSpeedSet = -300;//mm/s
    & V* ~& a$ c) w! F8 Y
25. 
    
26. int main(void)
    + ^- X) q) A7 E1 L
27. {
    
28. 
    2 c% ?& Z, A' G& K1 k1 |
29.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
    
30.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
    
31.         
    
32.         MY_NVIC_PriorityGroupConfig(2);        //=====设置中断分组
    
33.         
    
34.         delay_init();                            //=====延时函数初始化
    - w, }# x3 c4 y# f; j$ ]" l
35.         LED_Init();                     //=====LED初始化    程序灯        
    5 N  p# B8 H3 U$ I: m2 D& A
36.         
    
37.         usart3_init(9600);              //=====串口3初始化  蓝牙 发送调试信息
    & x( }7 h# N- r4 t9 w
38. 
    : r8 q4 k1 S! C! F1 x) O
39.         Encoder_Init_TIM2();            //=====初始化编码器1接口
    
40.         Encoder_Init_TIM4();            //=====初始化编码器2接口
    
41.         
    
42.         Motor_Init(7199,0);             //=====初始化PWM 10KHZ，用于驱动电机 如需初始化驱动器接口
    - d$ j% S' x" \& K
43.         
    / T6 n/ G7 l; @5 V0 v; K4 s; Z
44.         TIM3_Int_Init(50-1,7200-1);     //=====定时器初始化 5ms一次中断
    
45. 
    ; b6 w7 c' Z8 ]9 K. S6 z$ ]* t
46.         PID_Init();                                                //=====PID参数初始化
    
47.         
    5 a! P0 c' w) ?
48.         while(1)
    
49.         {
    ! p& P0 p! B* x
50.                 //给速度设定值和实时值赋值
    * d) h3 R: F, u9 {) |
51.                 pid_Task_Letf.speedSet  = leftSpeeSet;
    : v  t- n9 X- R1 f, K  M9 G
52.                 pid_Task_Right.speedSet = rightSpeedSet;
    
53.                 pid_Task_Letf.speedNow  = leftSpeedNow;
    1 |: Y0 I% k5 V5 y8 |( W. f; x
54.                 pid_Task_Right.speedNow = rightSpeedNow;
    # U8 D+ L6 R/ m) C- i9 T" W
55.                
    
56.                 //执行PID控制函数
    
57.                 Pid_Ctrl(&motorLeft,&motorRight);
    + S6 B/ l6 s4 r7 q1 O# s
58.                
    4 K. {" @3 U( F) c
59.                 //根据PID计算的PWM数据进行设置PWM
    
60.                 Set_Pwm(motorLeft,motorRight);
    
61.                
    ' m+ m, d4 p  r( i. e& F! s7 K
62.                 //打印速度
    
63.                 printf("%d,%d\r\n",leftSpeedNow,rightSpeedNow);
    
64.         }
    % Z. H5 f4 J' A+ h4 M. p
65. }
    
66. 
    
67. //5ms 定时器中断服务函数
    
68. 
    . O# F7 ]& o# S0 o1 h
69. void TIM3_IRQHandler(void)                            //TIM3中断
    
70. {
    2 {  d% o6 v" G+ @' b# D5 V
71.         if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否
    ' O5 ]4 ^9 [6 ~0 }- b; ?/ E5 e
72.         {
    
73.                 TIM_ClearITPendingBit(TIM3, TIM_IT_Update);   //清除TIMx的中断待处理位
    
74.                
    : w3 k* h5 X0 N/ {2 [
75.                 Get_Motor_Speed(&leftSpeedNow,&rightSpeedNow);//计算电机速度
    % v* ~. p; K) A, n6 n: E
76.                
    " m9 w& J2 R/ {6 \- Q; d6 O8 Z& t9 `
77.                 Led_Flash(100);                               //程序闪烁灯
    " K) Q. Z( K: P: C0 ~* Q9 p% [
78.         }
    
79. }

复制代码

2.3.4 总结

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

$ M$ O" b! H4 \" {/ ?: n/ K9 P7 j4 y6 ~

3 D7 @! B8 k& l! ~" F& G* Q" [: V$ d