2.3.1 解决的问题
* Y; x+ ]0 ?% {/ e
# ]& w8 _7 e) a+ e/ M& i5 S# J: \
# T8 w, i5 u$ K

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

2.3.2 PID理论
6 h- s" P. _+ D' v* i7 H% s

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

3 s3 [+ z) J0 t

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

) j, x# L# J9 Y5 ?6 I2 ~) Y

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

/ E% L* u) I  d

* Y5 q: q; L' d- s% g0 g

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

  }/ q, u& x; }6 s! c

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

6 H: T$ W/ C3 ~; z

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

1 [% Q1 ]0 j" ?! k' U7 H  F% G% `

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

3 Y4 N  t  v1 ]2 x

# x( X8 w& v7 L

2.3.3 代码分享

1 r) c& X. d: ^( u

(1）pid.h

1. #include "pid.h"
   
2. 
   
3. 
   ' A  i3 T7 [# W. s, a1 R
4. struct pid_uint pid_Task_Letf;
   
5. struct pid_uint pid_Task_Right;
   ! `8 x; x# w- _7 J* j; u
6. 
   
7. /****************************************************************************
   
8. *函数名称：PID_Init(void)
   
9. *函数功能：初始化PID结构体参数
   
10. ****************************************************************************/
    
11. 
    
12. void PID_Init(void)
    
13. {
    6 A9 O: z3 Y& I, h+ P, p
14. //乘以1024原因避免出现浮点数运算，全部是整数运算，这样PID控制器运算速度会更快
    
15. /***********************左轮速度pid****************************/
    
16.         pid_Task_Letf.Kp = 1024 * 0.5;//0.4
    ; {  V4 k% `: w( a2 W' ^9 w1 n1 x0 _
17.          pid_Task_Letf.Ki = 1024 * 0;        
    , L/ B. D6 b0 P* \" W9 ?  e+ I
18.         pid_Task_Letf.Kd = 1024 * 0.08;
    
19.         pid_Task_Letf.Ur = 1024 * 4000;
      a" k8 D  U% Y1 X6 N! S' s; v
20.         pid_Task_Letf.Adjust   = 0;
    ; m; s2 O4 w# H; X& 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);               
    $ R) n0 s' [9 ^
25. /***********************右轮速度pid****************************/
    : n3 G3 v* k% p$ P
26.         pid_Task_Right.Kp = 1024 * 0.35;//0.2
    
27.          pid_Task_Right.Ki = 1024 * 0;        //不使用积分
    " J* ?$ L0 w  N# u1 ^1 G  V( {  c
28.         pid_Task_Right.Kd = 1024 * 0.06;
    
29.         pid_Task_Right.Ur = 1024 * 4000;
    
30.         pid_Task_Right.Adjust   = 0;
    % [- L" U9 ^. Z1 B9 W0 P& \
31.         pid_Task_Right.En       = 1;
    ) s% S( v4 s, u( }. D1 V) \; k. m
32.         pid_Task_Right.speedSet = 0;
    " C) ~; R  w5 T( ~. l8 L
33.         pid_Task_Right.speedNow = 0;
    & k0 Y& H8 i  U" y( R) b' w
34.         reset_Uk(&pid_Task_Right);
    
35. }
    + A% c6 n, ^, Q  a$ t
36. 
    6 {: y8 `5 C7 H) H
37. /***********************************************************************************************
    
38. 函 数 名：void reset_Uk(PID_Uint *p)
    
39. 功    能：初始化U_kk,ekk,ekkk
    
40. 说    明：在初始化时调用，改变PID参数时有可能需要调用
    * M% m) K4 V" L# C% `7 w0 l
41. 入口参数：PID单元的参数结构体 地址
    
42. ************************************************************************************************/
    * ^4 x6 w2 v4 v. ^9 \
43. 
    % M' y/ w# I& x! q* M! u/ v1 t; i
44. void reset_Uk(struct pid_uint *p)
    
45. {
    
46.         p->U_kk=0;
    / X# G% s0 i; K8 N! l5 o
47.         p->ekk=0;
    1 W- O6 e( y# {% f& V& |1 U3 ^0 F
48.         p->ekkk=0;
      ^$ \. p% e1 V' U5 B9 ~
49. }
    8 C; F5 y" e" b( [4 r4 n. W  X
50. 
    & j$ Z# J  G% v2 \! z" `
51. /***********************************************************************************************
    
52. 函 数 名：s32 PID_commen(int set,int jiance,PID_Uint *p)
    
53. 功    能：PID计算函数
    
54. 说    明：求任意单个PID的控制量
    
55. 入口参数：期望值，实测值，PID单元结构体
    
56. 返 回 值：PID控制量
    
57. ************************************************************************************************/
    
58. 
    ! m7 E/ g! |- p9 e. g2 j* _: n9 |
59. s32 PID_common(int set,int jiance,struct pid_uint *p)
    9 G& m/ G5 U) F/ G
60. {
    0 b2 m4 Q2 @+ w+ u' w7 |
61.         int ek=0,U_k=0;
    
62. 
    
63.         ek=jiance - set;                                                               
    - M( R3 m9 w7 K6 s' w$ v
64.         
    
65.         U_k=p->U_kk + p->Kp*(ek - p->ekk) + p->Ki*ek + p->Kd*(ek - 2*p->ekk + p->ekkk);
    
66.         
    0 i! u* Z: N8 C* }
67.         p->U_kk=U_k;
    * }8 V7 J9 h8 G, z8 ~* z9 r9 P+ k* r$ j8 {
68.     p->ekkk=p->ekk;
    ) S2 _5 }6 R" i. ]6 L2 u
69.         p->ekk=ek;
    2 m  `( Q. L5 R& n
70.         
    
71.         if(U_k>(p->Ur))                                                   
    7 G  D! X8 j2 w; [! p3 i0 K5 Q# O
72.                 U_k=p->Ur;
    
73.         if(U_k<-(p->Ur))
    
74.                 U_k=-(p->Ur);
    1 {* u4 q9 ~) I* h1 d
75.         
    
76.         return U_k>>10;
    
77. }
    
78. 
    5 T& o6 x. n) {  ?" e! g
79. /***********************************************************************************
    
80. ** 函数名称 ：void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
    
81. ** 函数功能 ：pid选择函数              
    5 i# m4 w" V* L" ^# x+ M
82. ***********************************************************************************/
    . O- u  F, M  P! ~% p
83. 
    
84. void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
    
85. {
    % ?0 x$ y: v# i. w$ e( b
86.         /**********************左轮速度pid*************************/
    $ o( @, k, R- {+ P  H
87.         if(pl->En == 1)
    1 a. U2 Q( x. Q# P3 c5 d
88.         {                                                                        
    
89.                 pl->Adjust = -PID_common(pl->speedSet, pl->speedNow, pl);               
    7 m/ V4 G6 |+ C' w$ b% h$ L* ~! Y
90.         }        
    , ^+ \5 ?' {4 Y7 N
91.         else
    
92.         {
    9 |2 T, p% c9 }$ T; I
93.                 pl->Adjust = 0;
    1 @  v+ P4 C1 \1 J; D& j
94.                 reset_Uk(pl);
    + A! \  e2 q2 C$ y
95.                 pl->En = 2;
    3 e( Z4 ^! F+ q5 U+ n. v, U
96.         }
    
97.         /***********************右轮速度pid*************************/
    
98.         if(pr->En == 1)
    
99.         {
    ' N, b% M* S$ J1 L6 V/ w
100.                 pr->Adjust = -PID_common(pr->speedSet, pr->speedNow, pr);               
     % b: f3 y6 o. S+ T2 G6 I! h& N
101.         }        
     
102.         else
     ; d- g. x/ P. ^& ?  p
103.         {
     
104.                 pr->Adjust = 0;
     # M. ^' O/ c7 l* [, V$ E* h
105.                 reset_Uk(pr);
     % S, K2 m3 g; `$ q
106.                 pr->En = 2;
     * l( T& {) @% b! n7 v$ L/ [
107.         }
     1 F  I; q+ Q  h7 M+ d: u, \
108. }
     ; f# i1 ]2 d. ]7 a% [3 r- v  A
109. 
     
110. /*******************************************************************************
     
111. * 函数名：Pid_Ctrl(int *leftMotor,int  *rightMotor)
     
112. * 描述  ：Pid控制
     0 C4 \, o' O) F7 X
113. *******************************************************************************/
     
114. 
     5 f: D5 s, e8 z0 g" S
115. void Pid_Ctrl(int *leftMotor,int  *rightMotor)
     ! C( P/ g6 I+ b7 S
116. {
     7 V! K& _0 j* N2 `
117.         Pid_Which(&pid_Task_Letf, &pid_Task_Right);
     
118.         *leftMotor  += pid_Task_Letf.Adjust;
     
119.         *rightMotor += pid_Task_Right.Adjust;
     
120. }
     ( `  N  l5 e2 `

复制代码

0 |  J$ m, I! z: {

(2)main.c

1. #include "sys.h"
   . ~! @% d- X( U9 x
2. 
   0 @! E  L; ~$ f8 `  m; N
3. //====================自己加入的头文件===============================
   : O+ F% }9 M/ l( I/ r
4. #include "delay.h"
   
5. #include "led.h"
   
6. #include "encoder.h"
   ; }8 u9 b; J3 G" Y' L3 {
7. #include "usart3.h"
   ! o1 Q) r6 ?' ^
8. #include "timer.h"
   & j! H7 I. ~. Z( J$ I
9. #include "pwm.h"
   
10. #include "pid.h"
    
11. #include "motor.h"
    ( z# k# W) T5 t2 ?1 @( m+ w( J" u: O
12. #include <stdio.h>
    ) V: q0 M' ~* D
13. /*===================================================================
    + {- i7 e+ p7 U6 O- \. y# i6 G
14. 程序功能：直流减速电机的速度闭环控制测试
    3 ~& J7 i) q! q1 c7 I) `( `
15. 程序编写：公众号：小白学移动机器人
    
16. 其他    ：如果对代码有任何疑问，可以私信小编，一定会回复的。
    
17. =====================================================================
    8 q: z+ R% D  Q
18. ------------------关注公众号，获得更多有趣的分享---------------------
    ! N4 {5 Y( `& e( a3 q2 q- N
19. ===================================================================*/
    
20. int leftSpeedNow  =0;
    ! W4 v' s% V2 _7 |. u" L
21. int rightSpeedNow =0;
    
22. 
    - ]; w% {1 U$ V1 x  v
23. int leftSpeeSet   = -300;//mm/s
    8 {8 u1 |" b( m& d
24. int rightSpeedSet = -300;//mm/s
    
25. 
    
26. int main(void)
    
27. {
    " S: ~9 c2 _+ \8 D7 l
28. 
    - J) z5 ~5 q5 [1 Y  D5 T
29.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
    $ J! q7 I9 _1 E, H4 v
30.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
    
31.         
    2 A6 {+ X: ?" W- h  R  f
32.         MY_NVIC_PriorityGroupConfig(2);        //=====设置中断分组
    4 \9 l" [, y6 t* P- I
33.         
    & H; t7 P2 {* P& a$ W0 I
34.         delay_init();                            //=====延时函数初始化
    * a4 u. F- Q) w2 d
35.         LED_Init();                     //=====LED初始化    程序灯        
    ; R; B8 Y3 h  I1 k' K6 z8 \
36.         
    
37.         usart3_init(9600);              //=====串口3初始化  蓝牙 发送调试信息
    : J1 T/ E6 W. Y% B% w- i: ~* w
38. 
    , g6 L6 c3 v& j0 R, h
39.         Encoder_Init_TIM2();            //=====初始化编码器1接口
    
40.         Encoder_Init_TIM4();            //=====初始化编码器2接口
    
41.         
    
42.         Motor_Init(7199,0);             //=====初始化PWM 10KHZ，用于驱动电机 如需初始化驱动器接口
    2 M! |9 h/ h* D0 }) K2 a: V1 i
43.         
    6 V7 C4 y" M/ H6 q7 H7 O/ S
44.         TIM3_Int_Init(50-1,7200-1);     //=====定时器初始化 5ms一次中断
    ! K5 t6 a& q4 l- g- H) B% X
45. 
    3 }/ g& W, ?9 w7 F
46.         PID_Init();                                                //=====PID参数初始化
    ! Y" ]  S+ ]7 A. |
47.         
    
48.         while(1)
    # E. C8 U& A+ ?* A# W+ U0 [2 [( ^  N
49.         {
    : Q. i5 Q' i2 ^# k0 b5 Y" b
50.                 //给速度设定值和实时值赋值
    
51.                 pid_Task_Letf.speedSet  = leftSpeeSet;
    * P% u5 N, C% R
52.                 pid_Task_Right.speedSet = rightSpeedSet;
    
53.                 pid_Task_Letf.speedNow  = leftSpeedNow;
    
54.                 pid_Task_Right.speedNow = rightSpeedNow;
    2 M. y! O/ ?, o8 o! B( f2 C
55.                
    
56.                 //执行PID控制函数
    
57.                 Pid_Ctrl(&motorLeft,&motorRight);
      w- ]; J, z  L* E1 n0 K
58.                
    
59.                 //根据PID计算的PWM数据进行设置PWM
    
60.                 Set_Pwm(motorLeft,motorRight);
    - e7 K# T! w% k
61.                
    
62.                 //打印速度
    
63.                 printf("%d,%d\r\n",leftSpeedNow,rightSpeedNow);
    * y8 P2 E2 _" Z# i$ E; a6 I
64.         }
    * @( N$ y5 G, N& |( z
65. }
    + I5 O& y" F3 `  Q( M, z$ s
66. 
    7 y7 R: v# Z  k) p1 P# e
67. //5ms 定时器中断服务函数
    " W8 \( ]% M: v% S7 \
68. 
    
69. void TIM3_IRQHandler(void)                            //TIM3中断
    
70. {
    
71.         if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否
    
72.         {
    8 l6 I; q! Q4 B9 W( V6 ]+ y
73.                 TIM_ClearITPendingBit(TIM3, TIM_IT_Update);   //清除TIMx的中断待处理位
    
74.                
    - R& [/ C$ d' _0 @3 i1 j
75.                 Get_Motor_Speed(&leftSpeedNow,&rightSpeedNow);//计算电机速度
    
76.                
    
77.                 Led_Flash(100);                               //程序闪烁灯
    , S3 j( D7 M" L) u/ o5 m
78.         }
    
79. }

复制代码

1 ^3 F! L+ s  N$ p; v7 {* |" m& s1 D2.3.4 总结

: R; c" U- A8 O1 d6 a6 \

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

# J1 k, _- Q5 |