2.3.1 解决的问题
- |0 t, N% ^( m. u4 O) f

- i& c& J/ C% i3 P

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

8 t' @7 a( E. L" S0 [+ k

2.3.2 PID理论
6 J; J9 c3 B0 B5 @8 o9 T8 I- \" v
- a6 m: _) C: V

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

8 s7 ~1 p2 h. v* U/ s

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

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

' Q% J2 W# p1 _& L% _& ^

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

) \& s- t$ a" H* u; t

& \6 G- M' n- y' S

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

% [, C, Q- ]0 q& T% {

1 X9 i+ o. |8 y% t( E) A

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

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

0 H% @; R4 E! `- Q: M: o

6 Q& C8 Q5 Z9 \( q0 n+ ~* O6 M

2.3.3 代码分享

(1）pid.h

1. #include "pid.h"
   & [* ^, h# n# E# m0 D5 j
2. 
   
3. 
   
4. struct pid_uint pid_Task_Letf;
   
5. struct pid_uint pid_Task_Right;
   
6. 
   
7. /****************************************************************************
   7 E. y" T  l' r2 I. b
8. *函数名称：PID_Init(void)
   
9. *函数功能：初始化PID结构体参数
   
10. ****************************************************************************/
    % N: h* h% r: z; j% I1 Y7 }: z
11. 
    5 z* @. p0 `# P0 |0 k( z
12. void PID_Init(void)
    5 B; V3 L; S5 I
13. {
    
14. //乘以1024原因避免出现浮点数运算，全部是整数运算，这样PID控制器运算速度会更快
    
15. /***********************左轮速度pid****************************/
    % S" Z% @& S% ?8 B5 }& a" t
16.         pid_Task_Letf.Kp = 1024 * 0.5;//0.4
    : `2 k. Y6 {. f7 n+ Z0 q# M
17.          pid_Task_Letf.Ki = 1024 * 0;        
    1 N! C' x% }! i: A( i
18.         pid_Task_Letf.Kd = 1024 * 0.08;
    
19.         pid_Task_Letf.Ur = 1024 * 4000;
    8 U# j& O# J8 B4 I+ c& [
20.         pid_Task_Letf.Adjust   = 0;
    ; u* Q3 V2 }7 Y! B( A4 O+ L
21.         pid_Task_Letf.En       = 1;
    3 P, I( k, X1 ]) f7 Z; M
22.         pid_Task_Letf.speedSet = 0;
    8 H# c2 T6 C; P; a
23.         pid_Task_Letf.speedNow = 0;
    
24.         reset_Uk(&pid_Task_Letf);               
    
25. /***********************右轮速度pid****************************/
    6 T9 i% ^& P6 p, u& F
26.         pid_Task_Right.Kp = 1024 * 0.35;//0.2
    8 ?4 B. O# Z) ^" |4 D# Q! h
27.          pid_Task_Right.Ki = 1024 * 0;        //不使用积分
    8 ^) \7 C. O- _4 ]7 [
28.         pid_Task_Right.Kd = 1024 * 0.06;
    " r5 f9 B6 n: x5 v8 D: _" j; s6 ]
29.         pid_Task_Right.Ur = 1024 * 4000;
    7 F& I1 v2 c) ~( D+ ~
30.         pid_Task_Right.Adjust   = 0;
    " K6 g: T3 ?; J" Q; e# N
31.         pid_Task_Right.En       = 1;
    2 x4 |- t" D/ b- ]. U1 S
32.         pid_Task_Right.speedSet = 0;
    ; J+ m/ U( \' o# M0 |3 t
33.         pid_Task_Right.speedNow = 0;
    ) x: w8 ~5 g0 M$ |2 u
34.         reset_Uk(&pid_Task_Right);
    3 l  c3 w  ^  c6 H/ O% w" Q$ f0 D
35. }
    / M) V( ~7 T$ Q% a/ f4 t
36. 
    - y9 O! J- `) I" C$ v7 F' \
37. /***********************************************************************************************
    
38. 函 数 名：void reset_Uk(PID_Uint *p)
    
39. 功    能：初始化U_kk,ekk,ekkk
    % |, K6 N* a% Q2 l  X" F
40. 说    明：在初始化时调用，改变PID参数时有可能需要调用
    0 z  s7 d+ z; }6 k7 ~
41. 入口参数：PID单元的参数结构体 地址
    
42. ************************************************************************************************/
    
43. 
    & O7 f" F! W7 I* t/ s
44. void reset_Uk(struct pid_uint *p)
    9 k- B  B7 l0 m! `6 q) U
45. {
    6 d/ _3 G; l( g- v- Z
46.         p->U_kk=0;
    ! m' w4 N) I3 G1 Q9 w; J! H) j
47.         p->ekk=0;
    
48.         p->ekkk=0;
    2 |) j+ ~7 _: R- w
49. }
    1 u+ Q  N( r  K6 x  u
50. 
    7 G% }. z- _) d! G5 a
51. /***********************************************************************************************
    
52. 函 数 名：s32 PID_commen(int set,int jiance,PID_Uint *p)
    
53. 功    能：PID计算函数
    " G! m- Z5 x- P5 l, O
54. 说    明：求任意单个PID的控制量
    1 x) {/ n( t" U/ A& Z6 t: h0 o' P
55. 入口参数：期望值，实测值，PID单元结构体
      [: P5 a3 f; k* [8 C2 T, S) S8 F
56. 返 回 值：PID控制量
    
57. ************************************************************************************************/
    6 E. O% D$ y& y- P
58. 
    
59. s32 PID_common(int set,int jiance,struct pid_uint *p)
    
60. {
    
61.         int ek=0,U_k=0;
    
62. 
    
63.         ek=jiance - set;                                                               
    " t9 y" f6 c6 K% p' r
64.         
    
65.         U_k=p->U_kk + p->Kp*(ek - p->ekk) + p->Ki*ek + p->Kd*(ek - 2*p->ekk + p->ekkk);
    
66.         
    
67.         p->U_kk=U_k;
    
68.     p->ekkk=p->ekk;
    
69.         p->ekk=ek;
    . d+ o# Q) c2 F! i
70.         
    
71.         if(U_k>(p->Ur))                                                   
    9 ?! Q" N$ X3 M8 N& x# m
72.                 U_k=p->Ur;
    . E& ~, V, A' ]. Y. S( z- }
73.         if(U_k<-(p->Ur))
    
74.                 U_k=-(p->Ur);
    
75.         
    ' b8 x+ a+ I7 i
76.         return U_k>>10;
    
77. }
    
78. 
    / {5 a6 g0 i# s
79. /***********************************************************************************
    
80. ** 函数名称 ：void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
    ) U' g: G! w8 ^1 m2 u9 x
81. ** 函数功能 ：pid选择函数              
    
82. ***********************************************************************************/
    1 Z/ z& E& Y7 _
83. 
    2 l! l' }6 e* g# R0 u3 Z/ E
84. void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
    9 m  p+ F4 @0 E# {4 k. u# t
85. {
    2 m2 E: j% C! y* F
86.         /**********************左轮速度pid*************************/
    ! z+ ^1 ]& V% b: l! P( p
87.         if(pl->En == 1)
    
88.         {                                                                        
    
89.                 pl->Adjust = -PID_common(pl->speedSet, pl->speedNow, pl);               
    ( L' G5 ~9 U" A8 i) y! j7 e5 `; O
90.         }        
    2 l4 Q/ N+ `! G+ N6 j, w8 {
91.         else
    
92.         {
    
93.                 pl->Adjust = 0;
    . c- l$ K9 y9 ~4 V  X$ y0 q
94.                 reset_Uk(pl);
      n' l8 d. y6 G+ o  `
95.                 pl->En = 2;
    0 L1 `2 v: D9 u0 {6 p/ b5 T
96.         }
    ; w% F; v3 I: o1 e/ @9 s6 A0 c: q
97.         /***********************右轮速度pid*************************/
    
98.         if(pr->En == 1)
    
99.         {
    
100.                 pr->Adjust = -PID_common(pr->speedSet, pr->speedNow, pr);               
     . [9 E0 d  @4 m" F9 r4 Q5 l$ t
101.         }        
     
102.         else
     " d* Z8 D' A* P. g( m
103.         {
     2 y3 P& u8 b9 B! B0 a1 f) `
104.                 pr->Adjust = 0;
     0 M( o$ D9 Q8 N& P, n
105.                 reset_Uk(pr);
     . y; B, ~7 Y6 `  r9 U0 o  Z
106.                 pr->En = 2;
     
107.         }
     2 i2 u/ t/ m6 g, U: z, J1 }$ b
108. }
       F/ f$ v' H! x" n* ]
109. 
     
110. /*******************************************************************************
     
111. * 函数名：Pid_Ctrl(int *leftMotor,int  *rightMotor)
     9 X7 Z+ R. l3 q- D+ s9 F6 @) W6 |
112. * 描述  ：Pid控制
     * h7 W) N* k+ g) `
113. *******************************************************************************/
     4 j! z  r* M4 O9 B4 ^3 M* a
114. 
     5 @& m2 o/ g; i2 R+ j9 N
115. void Pid_Ctrl(int *leftMotor,int  *rightMotor)
     ) Z; n3 x0 L7 b! ^9 Z; b
116. {
     
117.         Pid_Which(&pid_Task_Letf, &pid_Task_Right);
     2 B9 r! r. D& @) ~
118.         *leftMotor  += pid_Task_Letf.Adjust;
     6 `* O5 v3 n& h1 j& n
119.         *rightMotor += pid_Task_Right.Adjust;
     
120. }
     7 t5 Y( u7 x- S+ D* H1 b" m

复制代码

4 ~3 u6 k. {  a3 K) D8 _+ U- ^$ D

(2)main.c

1. #include "sys.h"
   
2. 
   3 y( p& w6 }" \$ w% X$ y
3. //====================自己加入的头文件===============================
   ; W/ ?5 \4 `% o
4. #include "delay.h"
   3 [; Y- q1 {4 A) o% z
5. #include "led.h"
   
6. #include "encoder.h"
   * j. B, G' z% F' q3 K/ \
7. #include "usart3.h"
   
8. #include "timer.h"
   4 i5 y$ m/ z/ G
9. #include "pwm.h"
   
10. #include "pid.h"
    
11. #include "motor.h"
    
12. #include <stdio.h>
    " ?$ X4 d7 I' j& h4 w
13. /*===================================================================
    ; j0 @/ _2 W5 o' x+ s. ^
14. 程序功能：直流减速电机的速度闭环控制测试
    + O, A! P$ e9 j
15. 程序编写：公众号：小白学移动机器人
    ) P1 `# e* a8 t! R# b/ I
16. 其他    ：如果对代码有任何疑问，可以私信小编，一定会回复的。
    
17. =====================================================================
    ) T4 J) x6 C; D6 p& N2 l6 c* ?& d
18. ------------------关注公众号，获得更多有趣的分享---------------------
    4 g" M2 W' x& m
19. ===================================================================*/
    4 u" L1 K; j' w. d/ z( ]8 F' Y* q4 k8 E
20. int leftSpeedNow  =0;
    
21. int rightSpeedNow =0;
    
22. 
    
23. int leftSpeeSet   = -300;//mm/s
    
24. int rightSpeedSet = -300;//mm/s
    / p) I  t  F  x0 `9 x' ^" M$ H2 P
25. 
    ! [: k  O+ l0 S9 s. K( L* K
26. int main(void)
    # d' H" n8 _* ]5 b" W
27. {
    ' `3 k5 F) J! V
28. 
    1 G' ~! T6 r! f* c) n
29.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
    
30.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
    0 e9 B( n. v$ A3 s; X0 k# s% W
31.         
    " D6 ?% c9 ]% i
32.         MY_NVIC_PriorityGroupConfig(2);        //=====设置中断分组
    6 \8 t  J" ^/ R: g: [, u) o# F
33.         
    ; F9 O( }; F; Y* G$ }
34.         delay_init();                            //=====延时函数初始化
    
35.         LED_Init();                     //=====LED初始化    程序灯        
    3 G- y! o. H* v+ `/ E3 W! @
36.         
    / k" t- `0 t2 ^
37.         usart3_init(9600);              //=====串口3初始化  蓝牙 发送调试信息
    + g& k9 a. t- D) u8 `( N9 ]
38. 
    3 N8 Q$ d! o1 w; h; {
39.         Encoder_Init_TIM2();            //=====初始化编码器1接口
    2 E7 `( T& f0 s- r
40.         Encoder_Init_TIM4();            //=====初始化编码器2接口
    
41.         
    
42.         Motor_Init(7199,0);             //=====初始化PWM 10KHZ，用于驱动电机 如需初始化驱动器接口
    : Q+ e+ M$ g! J
43.         
    
44.         TIM3_Int_Init(50-1,7200-1);     //=====定时器初始化 5ms一次中断
    7 P9 g. M. S3 i  C' X# R9 d
45. 
    
46.         PID_Init();                                                //=====PID参数初始化
    
47.         
    # O* Q1 m1 z/ K. }6 j8 t; `- c
48.         while(1)
    
49.         {
    : n+ T$ K* x4 t. B1 p
50.                 //给速度设定值和实时值赋值
    
51.                 pid_Task_Letf.speedSet  = leftSpeeSet;
    ! j- E- q. C5 U3 ~
52.                 pid_Task_Right.speedSet = rightSpeedSet;
    
53.                 pid_Task_Letf.speedNow  = leftSpeedNow;
    2 w$ Z# g  [7 \! t
54.                 pid_Task_Right.speedNow = rightSpeedNow;
    3 k9 r5 F" x- N) {3 [
55.                
    
56.                 //执行PID控制函数
    5 c7 p* b& e) E" Z# ~
57.                 Pid_Ctrl(&motorLeft,&motorRight);
    
58.                
    
59.                 //根据PID计算的PWM数据进行设置PWM
    
60.                 Set_Pwm(motorLeft,motorRight);
    
61.                
    
62.                 //打印速度
    , L* @' O2 u# I, W. P* T, i- G8 R
63.                 printf("%d,%d\r\n",leftSpeedNow,rightSpeedNow);
    
64.         }
    - J9 @. d$ F' U* t7 |
65. }
    
66. 
    " d; w) L) v6 C! N* m+ D3 O. l
67. //5ms 定时器中断服务函数
    9 z( p# R8 @- I8 ?% M- k3 e7 Y
68. 
    
69. void TIM3_IRQHandler(void)                            //TIM3中断
      I, |) }7 p4 r' r8 y* |: o- H
70. {
    3 t* b1 D  C' T7 Y" j
71.         if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否
    
72.         {
    # b/ h' i5 L1 g3 t
73.                 TIM_ClearITPendingBit(TIM3, TIM_IT_Update);   //清除TIMx的中断待处理位
    * D9 ]5 a& D# D; l9 r5 G
74.                
    
75.                 Get_Motor_Speed(&leftSpeedNow,&rightSpeedNow);//计算电机速度
    
76.                
    
77.                 Led_Flash(100);                               //程序闪烁灯
    ! Q6 ^0 g8 P, o
78.         }
    ) z' A! F. q2 s9 ^
79. }

复制代码

/ W) L4 E. E9 h! r0 S
2.3.4 总结

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

! t! f1 c; v9 a$ C
* e* r) B* E/ F% `/ b% t  O: l

* E' ?7 X& g& Y% @+ q- P