概述
小车外形：

# h& E1 F: T# ]# }& o5 H' s# B
" J) T/ ?4 r4 N
# L  C, {# ?* C3 U7 k0 m, p( g功能简介
利用摄像头识别前车尾部的AprilTag，得到前车位置，传回stm32主控板处理，使两车在行驶时保持恒定距离，实现自动跟车。
& p( z& I0 p9 P1 b9 ~3 L5 P

7 @0 t  e& V; \5 @

4 ^0 C2 R, w% ~& b1 AopenMV4摄像头
1.1 Apriltag识别与串口传输

# E& V$ l7 ^& y9 [9 Q/ Q
3 m! O7 m2 h0 I; h. AAprilTag是一个视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志（与二维码相似，但是降低了复杂度以满足实时性要求），可以快速地检测标志，并计算相对位置。
6 N$ @6 I7 b2 A7 w2 N0 V1 M

Apriltag示例：

通过识别Apriltag，可以得到x,y,z三个方向的距离以及偏移角度。这里只需要三维的距离即可，通过串口传回stm32.

1. <font face="微软雅黑" size="3">
   
2. import sensor, image, time, math,pyb
   
3. from pyb import UART
   
4. 
   ' @; `0 [  C/ A4 t; r# r
5. sensor.reset()
   ! h" X3 e" j& D7 F3 `
6. sensor.set_pixformat(sensor.RGB565)
   
7. sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
   - a. \# f  D3 Y. M" P/ Y  {4 G
8. sensor.skip_frames(time = 2000)
   " \2 u) o- v2 _  P7 P0 y) ]1 j
9. sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
   8 m( i- e( V  R, S% a6 H; D; w! G
10. sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
    
11. clock = time.clock()
    . U! L0 p4 m/ ]1 Q# E: R" H
12. uart = UART(3, 115200)#串口波特率
    : G7 G/ u8 I, P: K, m
13. 
    0 h  q( r, v: _
14. f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
    / v" H# M" p, T8 V. H
15. f_y = (2.8 / 2.952) * 120 # find_apriltags defaults to this if not set
    6 n" a8 {7 j2 m" h) h; }, F
16. c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
    
17. c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
    
18. 
    # s5 b. a5 _; O  t7 m7 H- k
19. def degrees(radians):
    
20.     return (180 * radians) / math.pi
    
21. 
    & n2 I2 d  Q' J, l8 `
22. while(True):
    
23.     clock.tick()
    6 {) ^. [3 E# |( n6 `" t
24.     img = sensor.snapshot()
    
25.     for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11
    
26.         img.draw_rectangle(tag.rect(), color = (255, 0, 0))
    
27.         img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
    ) k. I& `9 o0 R' t9 h  T1 C7 x$ w
28.         print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
    7 `0 `! s# J9 `& {
29.             #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
    : C2 S% e1 ]- a# [1 [
30.         # Translation units are unknown. Rotation units are in degrees.
    
31.        # print("Tx %f, Ty %f, Tz %f" % print_args)
    
32.         uart.write("A%.2f,B%.2f,C%.2f," % print_args+'\r\n')#设置特定格式，以便于stm32分割取得数据
      G- H* C* [' F* n* L
33.         #pyb.delay(500)
    
34.    # print(clock.fps())</font>

复制代码

STM32主控板（型号为F407）
& {0 B; f6 N$ f4 S6 a3 O+ ^
- B! U/ {1 H! a
2.1 时钟与中断配置

+ M2 A( s6 l) t2 Y, ^
" O6 R; X) [2 ~: Q2 L' k( W5 w* N附上stm32时钟示意图：
: T4 |! I+ H! v1 [9 S; _2 B* h
2 _' L& O" E* q$ t3 I8 h6 C定时器示意图：

定时器分配：
  B: ~+ k7 q$ @6 r- F, j: `, [
所有时钟初始化的函数：（每个函数的详细内容在后面）
' q7 m; e' Z0 D5 e% l. D4 @7 M' p

1. <font face="微软雅黑" size="3">
   
2. TIM8_PWM_Init(400-1,20-1);  //用于控制电机，168M/20=8.4Mhz的计数频率,重装载值400，所以PWM频率为 8.4M/400=21Khz.
   0 N& Z8 e  ?/ }& C$ G
3.   TIM3_PWM_Init(200-1,8400-1);//用于控制舵机，50HZ
   
4.   TIM2_Int_Init(400-1,20-1);//定时中断，21KHZ
   5 r$ \+ |6 h' g. n
5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数，20HZ，50ms中断一次
   % v7 f( p* o  A6 v0 P8 e
6.   uart_init(115200);  //初始化串口1波特率为115200
   
7.   Encoder_Init_TIM4();//编码器接口初始化</font>

复制代码

2.2 串口收发与数据处理

9 v- w* T9 l# _1 K0 J
串口中断：USART1，USART2
6 O# {% b7 O) k0 x' a& M串口初始化函数（以USART1为例）：
; p: Z2 \/ A4 S$ ^

1. <font face="微软雅黑" size="3">
   
2. void uart_init(u32 bound){
   
3.    //GPIO端口设置
   
4.   GPIO_InitTypeDef GPIO_InitStructure;
   
5.   USART_InitTypeDef USART_InitStructure;
   
6.   NVIC_InitTypeDef NVIC_InitStructure;
   1 O0 e1 O7 v  u7 T3 \+ D
7.   
   
8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
   
9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
   
10. 
      V" D$ F2 {: I- s7 E" d
11.   //串口1对应引脚复用映射
    : P0 |1 E7 g/ y
12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
    
13.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
    7 i( s( g4 i4 J
14.   
    " P8 h% x5 b2 m$ a
15.   //USART1端口配置
    
16.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
    5 P* B0 Q8 f  n5 I% u0 c1 @
17.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    ( Q. ]; Z# i$ `' I
18.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度50MHz
    
19.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
    
20.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
    4 a7 j" l) {# ~+ D, L
21.   GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9，PA10
    6 t" E+ F* o( X+ X) @8 O0 b6 c# l  Q
22. 
    ( ?' W* S$ O! `# ~) M; I0 t* \  D
23.    //USART1 初始化设置
    
24.   USART_InitStructure.USART_BaudRate = bound;//波特率设置
    
25.   USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
    
26.   USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
    
27.   USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
    5 n, T4 l( Y5 x/ X5 F* K4 `6 f
28.   USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
    
29.   USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;  //收发模式
    3 K% H% W- X5 `0 P" w; u  f
30.   USART_Init(USART1, &USART_InitStructure); //初始化串口1
    
31.   
    
32.   USART_Cmd(USART1, ENABLE);  //使能串口1
    5 I, [9 f3 {" ^& ^! d; `
33.   
    
34.   USART_ClearFlag(USART1, USART_FLAG_TC);
    + u2 E9 a5 m) c5 _) C
35.   
    
36. #if EN_USART1_RX  
    ' m8 O' S% S  [& h$ H, Y
37.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
    
38. 
    9 P/ p) x  J# U" P2 L) n
39.   //Usart1 NVIC 配置
    
40.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
    
41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
    8 O, U, f, w$ f6 ]
42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
    
43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
    9 p- V' O0 a" ]1 Z/ l/ s
44.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、
    
45. 
    
46. #endif
    
47.   
    , L+ |! j) K5 E  e8 x6 w/ f! M
48. }</font>

复制代码

串口中断处理函数：

1. <font face="微软雅黑" size="3">
   
2. void USART1_IRQHandler(void)                  //串口1中断服务程序
   
3. {
   5 j* i' `0 p; c3 n
4.   u8 Res;
   
5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
   
6.   OSIntEnter();   
   
7. #endif
   
8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
   ( O$ ?/ q5 U0 Z* }  y8 `2 O, i
9.   {
   1 \: O# M  g; V4 q3 j* u$ d
10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
    
11.    
    3 x! b! N) a9 r3 H1 Z& i# c" W& V
12.     if((USART_RX_STA&0x8000)==0)//接收未完成
    3 s  r  t' z# z1 v
13.     {
    
14.       if(USART_RX_STA&0x4000)//接收到了0x0d
    7 W$ \" Y8 ^  z! s
15.       {
    
16.         if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
    / b+ D3 _& ?: D, |4 V
17.         else USART_RX_STA|=0x8000;  //接收完成了
    
18.       }
    
19.       else //还没收到0X0D
    
20.       {  
    : i8 }7 O9 d$ @5 q; Q  `# o* ]
21.         if(Res==0x0d)USART_RX_STA|=0x4000;
      p$ T' V; f: H' g! `. H
22.         else
    ' l& H6 }* Y6 G# o0 c" h+ \
23.         {
    . D2 s4 J) q$ D: {
24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
    
25.           USART_RX_STA++;
    9 S0 k( ]) p; N
26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
    0 B/ t& d8 q* ~
27.         }     
    2 f. c) J" x: V5 K+ P9 F! x
28.       }
    % D- ?, B0 g; P% e: w& ]' b7 L
29.     }        
    , w/ ?  f1 p8 @9 U4 x5 H
30.   }
    6 ]' d4 E! a, N0 v; G
31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
    
32.   OSIntExit();                        
    1 D8 E  i+ @' ]4 X. g9 K: U
33. #endif
    ! w* }; y" u* m- g/ ~
34. }
    
35. #endif  </font>

复制代码

字符串接收与处理（从openMV接收到的数据）：

1. <font face="微软雅黑" size="3">/*涉及到的全局变量
   & [( p5 H' y2 \8 u" v. a
2. float data[3];//x,y,z方向的距离，浮点数形式
   
3. unsigned char data_string[3][7];//x,y,z方向的距离，字符串形式
   
4. */
   8 j+ r; E6 Y" U# S/ c3 d# l- c
5. if(USART_RX_STA&0x8000)
   
6.     {  
   
7.       //清空字符串
   
8.       for(i=0;i<2;i++)
   - n) f8 K1 G! J) p% U. y% P/ k0 D
9.       {
   6 n& w  F3 k" W  B( H. @8 l3 U
10.         for(j=0;j<6;j++)
    
11.         {
    8 M2 h5 ^& h# d6 w# @( [" u
12.           data_string[i][j]=' ';
    0 }5 L) Z8 ?# {& L  S
13.         }
    
14.       }
    
15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
    
16.       for(t=0,j=0;t<len;t++)
    
17.       {
    
18.         j=0;
    $ m7 Z6 o% }9 n2 F$ z* C$ U
19.         if(USART_RX_BUF[t]=='A')
    ! e0 x5 @5 P: a2 \3 D! m
20.         {
    
21.           t++;//去掉首字母
    
22.           while(USART_RX_BUF[t]!=',')
    4 z( P0 Q) J, Q/ Y' h
23.           {
      ?  ?  I- e3 e! T) Y
24.             data_string[0][j]=USART_RX_BUF[t];
    
25.             t++;
    
26.             j++;
    4 N: Z7 N. }- ~' U! e! N- R
27.           }
    
28.         }
    0 k! b) G* q# n+ F+ Q4 @7 w3 I
29.         if(USART_RX_BUF[t]=='B')
    8 C( Z+ W- h4 K: v( Q
30.         {
    
31.           t++;//去掉首字母
    * W+ M% m. S( B' _1 K
32.           while(USART_RX_BUF[t]!=',')
    9 D$ H& p- s. B/ I' V1 O% V3 w9 b
33.           {
    
34.             data_string[1][j]=USART_RX_BUF[t];
    
35.             t++;
    1 Z8 R( R& J- C8 N' f
36.             j++;
    
37.           }
    5 }6 ], S- }' a. |/ G- A
38.         }
    
39.         if(USART_RX_BUF[t]=='C')
    ; o& o' _6 J0 ^! _1 u* N0 y
40.         {
    : l7 K8 B0 Q9 ~; `/ J
41.           t++;//去掉首字母
    2 `/ J! s% N( W4 K
42.           while(USART_RX_BUF[t]!=',')
    4 u/ O; r7 O7 N9 k8 Q6 C' @
43.           {
    # t& k1 e( f4 i, w  {; y" J+ p4 B5 K
44.             data_string[2][j]=USART_RX_BUF[t];
    
45.             t++;
    3 K, k+ V: @% `
46.             j++;
    
47.           }
    
48.         }
    ( f' \9 L/ W) n/ d
49.       }
    ' I! @! B  _1 H4 q! [% |) ]5 Z  C
50.       //把字符串转化为浮点数
    & j' p0 _  F+ c
51.       data[0]=myatof(data_string[0])/100.0;//x
    
52.       data[1]=myatof(data_string[1])/100.0;//y
    
53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z，输入是负数，转换为正
    $ _% W5 O4 ^* J, [& Y0 H" ^
54.         //USART2发送数据
    ; W9 k; T/ a) K9 H% G8 W9 s
55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
    - Q0 `* g, n8 ^5 \. N7 Z2 J
56.         
    4 L) {+ @8 {: D' o! L3 d) l
57.       //LCD更新显示
    3 Z8 ?" b4 i2 @* l. b) u0 R
58.       //显示xyz
    ! b$ Q& [8 A) V3 `
59. //        CLEAR(10);
    ' |( }# L; b6 s2 K" y
60. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);
    
61. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,data_string[0]);
    
62. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,clear);
    $ V1 N4 Z$ m6 W4 X
63. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
    & g) j+ N) G8 ?+ `5 Y
64. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
    8 }# B, Y1 X7 J0 T; s: ]) I( D
65. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);
    
66.         
    
67.         USART_RX_STA=0;//清除标志位
    
68.     }
    
69. }
    . N  |3 U  M& E- C" N6 J
70. </font>

复制代码

字符串转化为两位小数浮点数（用于后续PID控制）:

9 X2 Z5 x# L. C8 Y, V9 S. Q

1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数，返回的是乘100后的int值，因float返回有错误
   8 @) [$ V1 b; ?% t) Q3 y
2. {
   ; i% e1 z' `! O
3.   int flag = 1;//表示正数
   
4.   int res =0;
   / y# N# x" D$ G! F3 h, A3 P3 W
5.   u8 i=1; //小数点后两位
   
6.   while(*str != '\0')
   : h! Z+ @* z/ c$ T; w3 l( L% V. y
7.     {
   9 t, L% z6 F0 P: |: v/ P6 n0 c
8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
   
9.       {
   
10.         str++;
    
11.         continue;
    
12.       }
    * D( t, M/ L4 S4 P( h
13.       if(*(str-1) == '-')
    
14.       {
    + ]4 M. E3 N5 E- U2 g) u6 w4 D4 j
15.         flag=-1;//表示是一个负数
    + |3 q" m% w' [+ M& u& c
16.       }
    - I" V$ a  [1 _1 U) w1 E
17. 
    5 Q  v6 D5 a' X) l3 b# G* W
18.     while(*str >= '0' && *str <= '9')
    
19.     {
    ) K1 z1 l1 `$ D- ?9 b( F9 Y
20.       res = res *10 + (*str - '0');
    
21.       str++;
    ' i$ h/ u) Z( a" q3 H: D
22.     }
    , X3 T. e+ \8 h9 y8 a4 n# N
23.     if(*str == '.')
    
24.     {
    
25.       str++;
    . ?: ]! W* Y) H
26.       res = res *10 + (*str - '0');
    
27.       str++;
    7 D9 c, b  p1 T! L. i/ G; ]
28.       res = res *10 + (*str - '0');//保留两位，故加两次
    ; \" G+ Q# g. o0 [$ Z( v6 M
29.       return res*flag;
    0 O2 v3 b0 p+ G  ^, H- e
30.     }
    
31.     }
    ; }3 G' b6 L* _4 `" J
32. }
    
33. 
    
34. 
    
35. 2.3 LCD显示模块
    $ I5 u$ ^" _6 r7 }& n! X: K
36. 
    5 S+ Z; S7 D8 }* u  k
37. LCD模块用于调试时观察数据，调试完成可以删去，因为显示屏很耗时，使处理速度变慢
    
38. 驱动函数总览：
    
39. 
    
40. 
    : |# x  A2 |- F. w/ f
41. 
    
42. void LCD_GPIO_Init(void);
    4 D+ }' j2 P/ c. u5 N
43. void Lcd_WriteIndex(u8 Index);
    9 ^9 h" _- S9 Z$ w# [. s' `
44. void Lcd_WriteData(u8 Data);
    + A3 C# b, l- m& n* Z3 E
45. void Lcd_WriteReg(u8 Index,u8 Data);
    % j; C) w/ k6 W/ t
46. u16 Lcd_ReadReg(u8 LCD_Reg);
    
47. void Lcd_Reset(void);
    
48. void Lcd_Init(void);
    
49. void Lcd_Clear(u16 Color);
    
50. void Lcd_SetXY(u16 x,u16 y);
    & `, a( K! h4 C7 u* L! [9 C
51. void Gui_DrawPoint(u16 x,u16 y,u16 Data);
    
52. unsigned int Lcd_ReadPoint(u16 x,u16 y);
    . X7 ^) x$ n$ a
53. void Lcd_SetRegion(u16 x_start,u16 y_start,u16 x_end,u16 y_end);
    
54. void LCD_WriteData_16Bit(u16 Data);</font>

复制代码

: B5 v, ^, i3 y$ Y, P5 @
( s" u* Q/ t: W/ L* S/ `TFT屏幕初始化：
& K( {5 M- e  l$ b* y/ h
0 o7 c7 _+ N" T& ?( C

1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
   
2. {
   ' O! `/ C% _4 B) r0 i! E
3.   Lcd_Clear(WHITE);
   % G3 L7 W) Z* }1 H$ v
4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
   
5.   delay_ms(1000);
   3 K* u* p; H' `+ i5 f8 T
6.   Lcd_Clear(WHITE);
   
7.   Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
   : R! c0 X# t/ C* U: v
8.   Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:");
   
9.   Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:");
   % q9 E5 u2 Z3 }! }* e# K
10.   Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");
    ; A3 `  d; g5 h# v( l2 V: o
11. }</font>

复制代码

字符串显示函数;
4 i5 R+ Z$ ~7 u% I! c" z

1. <font face="微软雅黑" size="3">
   . h/ [+ x% u0 {! n
2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
   , `, |7 d; a, b
3. {
   3 ~2 m1 h1 G, m; y3 A. @
4.   unsigned char i,j;
   / `1 i% X# K" x( k, d
5.   unsigned short k,x0;
   % y  k3 p, u8 Z: T4 H2 _6 f
6.   x0=x;
   
7. 
   
8.   while(*s)
   $ m1 ^" a2 I8 ]* i
9.   {  
   * Q: O$ \& D" {! C
10.     if((*s) < 128)
    
11.     {
    
12.       k=*s;
    
13.       if (k==13)
      K6 y: w/ q* M. I7 w& U% u/ g
14.       {
    ! H) g3 }7 y: l+ S7 @6 C
15.         x=x0;
    & [; l" ?9 {  _
16.         y+=16;
    & o, t! ^3 J- `3 b
17.       }
    
18.       else
    
19.       {
    % c! M' J2 |. E9 G! g) w
20.         if (k>32) k-=32; else k=0;
    
21.   
    / v  r& Y1 m- E
22.           for(i=0;i<16;i++)
    
23.         for(j=0;j<8;j++)
    
24.           {
      D+ A5 F& J9 Q0 x* j
25.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    & \0 i' N1 u& b6 g* ^  ?' Q
26.             else
    5 L' Z8 o! |+ u- v& o  F9 |! k
27.             {
    
28.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    " {$ ~$ s: R3 Q% i0 V/ O
29.             }
    
30.           }
    
31.         x+=8;
    # [6 @; Z; u! _. ~2 u) J
32.       }
    
33.       s++;
    
34.     }
    
35.       
    1 x4 L% Z' O9 q+ _" U% J/ z" U
36.     else
    
37.     {
    1 q0 D7 H% G# R3 A% i
38.    
    + A! o1 \+ H( [9 K, B1 w& V
39. 
    
40.       for (k=0;k<hz16_num;k++)
    ' X6 r" v. Y  P
41.       {
    4 [0 ~: G! R9 R2 d  K& q! I
42.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
    # N% o6 v0 O% |9 \
43.         {
    2 o, d, g( Y2 ^8 E2 @5 i
44.             for(i=0;i<16;i++)
    ) b2 U* P9 c( n  q
45.             {
    7 L0 o5 K  M" c! C+ C
46.             for(j=0;j<8;j++)
    
47.               {
    
48.                   if(hz16[k].Msk[i*2]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    % w8 ~/ D& p) `, l
49.                 else {
    9 J2 T! i# a5 h, E0 o# t8 Z% ~% ?5 N
50.                   if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
51.                 }
    
52.               }
    
53.             for(j=0;j<8;j++)
    6 ]# Z/ p3 k9 A" O1 v: \/ H
54.               {
    
55.                   if(hz16[k].Msk[i*2+1]&(0x80>>j))  Gui_DrawPoint(x+j+8,y+i,fc);
    
56.                 else
    
57.                 {
    
58.                   if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);
    
59.                 }
    
60.               }
    
61.             }
    . l3 a; m- {6 q( _
62.         }
    
63.         }
      [# Z0 z1 a( i+ j: S/ j( {
64.       s+=2;x+=16;
    : |" T/ F9 i& N% k1 S
65.     }
    
66.    
    
67.   }
    3 T4 J, w( J9 P3 o3 v( ^- X
68. }
    ) Y$ I% h$ D2 R6 m* |: O
69. </font>

复制代码

2.4 电机、舵机与编码器
# T) i/ K2 y' U+ B
' g5 l( W3 F8 M/ M$ ~/ |' j
8 f. Z1 R- b4 |0 N定时中断：TIM2，用于修改电机和舵机的PWM占空比
; B6 ^4 [% x4 r初始化函数：
9 t+ H6 E. G1 @, o2 o+ L/ x7 b' i

1. <font face="微软雅黑" size="3">
   ( p# w8 ?6 D& A6 m
2. //通用定时器2中断初始化
   
3. //arr：自动重装值。
   8 p; k6 K- ?! @' i7 I* }
4. //psc：时钟预分频数
   / e( n0 f1 q; t7 D  g5 Z
5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
   
6. //Ft=定时器工作频率,单位:Mhz
   
7. //这里使用的是定时器2!
   
8. void TIM2_Int_Init(u16 arr,u16 psc)
   ; V6 U- a3 W8 [; V! g4 `; E
9. {
   
10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    
11.   NVIC_InitTypeDef NVIC_InitStructure;
    4 a8 R( [. J8 p3 W% I
12.   
    
13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
      M6 c' M$ @) y- n* |( ?6 P
14.   
    
15.   TIM_TimeBaseInitStructure.TIM_Period = arr;   //自动重装载值
    
16.   TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    
17.   TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
18.   TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
19.   
    ( O8 u. r0 i  }+ P/ J4 t
20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
    
21.   
    7 s- L/ C% g$ n
22.   TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断
    
23.   TIM_Cmd(TIM2,ENABLE); //使能定时器2
    ; r: m; g; o7 O2 B4 _. s
24.   
    # `' J2 o2 ?5 A  |# g1 ^
25.   NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器2中断
    
26.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级1
    
27.   NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
    & m$ C, l  u% Z' D* ~! `$ V) t
28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    
29.   NVIC_Init(&NVIC_InitStructure);
    
30. 
    - O- b$ s# U! N- i1 H
31. }</font>

复制代码

TIM2中断处理函数：

1. <font face="微软雅黑" size="3">
   
2. void TIM2_IRQHandler(void)
   & J9 a# S  d! C
3. {  
   
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
     l- w0 k: F5 n
5.   {
   ! O5 {0 |: c4 ^" t
6.     if(motor_flag==1)//反转
   
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   & W5 I  Z8 _6 d% }( W1 H
9.       TIM_SetCompare2(TIM8,0);
   
10.     }
    
11.     if(motor_flag==0)//正转
    # `% `! ?! |! K! {, g. |% M" Y
12.     {
    
13.       TIM_SetCompare1(TIM8,0);
    
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    9 M- q9 v8 R& U# q
15.     }
    
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，引脚：PA6
    
17.   }
    , ^( U  r, o  ?% [' D% a
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    
20. </font>

复制代码

PWM输出：TIM3（舵机），TIM8（电机）
初始化函数（以TIM8为例）：
3 v( d6 x+ @) p7 B$ `
/ |9 I; ]  U# o" H0 G! B. q

1. <font face="微软雅黑" size="3">
   
2. void TIM8_PWM_Init(u32 arr,u32 psc)
   
3. {               
   : ~& b* m& q+ V. S
4.   //此部分需手动修改IO口设置
   
5.   
   
6.   GPIO_InitTypeDef GPIO_InitStructure;
   * r( F9 \) A5 `1 D; Y+ l0 }% ^
7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   / b/ m+ H) s$ Y, }
8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
   $ O9 ]$ Z. q# r/ S* g/ l
9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
   2 b. _, w+ w% T  }# C8 o
10.   
    
11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
    
12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
    
13.   
    
14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
      G  w1 S7 F) j) C# X  M& [
15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    ; Z% ^" i: ]) D2 v# |) o6 o9 y
16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
    8 i  o% z3 b5 E: n& ?8 L
17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    . F! O9 |5 e; J+ H
18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
    1 `  b7 O1 d# f- }
19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7
    
20. 
    ; ^1 ~# W- [/ {2 ^$ }6 p' e
21.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    ; X; r2 A8 N9 B$ {! }2 c
22.   GPIO_Init(GPIOB,&GPIO_InitStructure);
    
23.   
    # ^0 K5 ~6 u% _# Y2 I+ }# W
24.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
    4 g1 n7 D  ~- z2 @
25.   GPIO_Init(GPIOC,&GPIO_InitStructure);
    
26.   
    ; q6 {- I5 Z4 Y) Z& d2 l
27.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
    1 l7 ]9 `' M' S$ I! I% ?8 {
28.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1
    
29.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
    6 ?( K0 s2 E' a9 {
30.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13复用为定时器1
    ( \) u. A& b  i* w7 N
31.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14复用为定时器1
    
32.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15复用为定时器1
    
33. 
    
34.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
    5 v& d; h0 C% E* s8 Z7 y5 P
35.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
36.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
    - _# ^9 C& h) V) d6 Q: ~
37.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
38.   
    
39.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
    
40.   
    3 D$ j' ~, b  d9 D4 z# ?, ^6 P
41.   //初始化PWM模式   
    & a  x5 o# s# [$ d+ w) @* G
42.   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    
43.   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    
44.   TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    
45.   TIM_OCInitStructure.TIM_Pulse = 0;
    
46.   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    6 R2 e. D7 w* m7 O. E0 Q
47.   TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
    1 K% S: R0 N7 l3 V  }
48.   TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
    
49.   TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
    + b& i7 w  \6 l
50. 
    8 ]# U' }$ e: i6 _
51.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    ( N5 c1 Y+ e& Z
52.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    7 G' @0 H$ N3 o6 v  c
53.   TIM_OC3Init(TIM8, &TIM_OCInitStructure);
    6 ?. e; f1 }3 [- g# ]
54. 
    $ H6 Z. }( y6 Z% M; p- a+ Z
55.   TIM_OC1PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
56.   TIM_OC2PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
57.   TIM_OC3PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
58. 
    
59.   TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
    
60.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
    3 s, x4 I9 a! ~: g* V
61.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
    ' z# n% i2 d+ `5 T% q3 I! S- J
62.   TIM_BDTRInitStructure.TIM_DeadTime = 0;
    - v) ~' h& j* c) X
63.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
    6 R( S' o' W" \0 l- L6 h# ?
64.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;
    
65.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
    ; K  l$ T/ \( [1 O# ^) N1 p( k
66.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);
    
67. 
    
68.   TIM_Cmd(TIM8,ENABLE);
    
69.   TIM_CCPreloadControl(TIM8,ENABLE);
    % F0 n* H0 S1 I! N. ~
70.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
    0 W* M) d. U- ]& z: w  Y' o
71.                      
    
72. }</font>

复制代码

编码器初始化函数：

1. <font face="微软雅黑" size="3">
   9 r+ c% r$ R# T  I( a
2. void Encoder_Init_TIM4(void)
   , I$ x# L' o* m/ O" G
3. {
   
4.     GPIO_InitTypeDef         GPIO_InitStructure;
   
5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   ' [) G& P8 n4 G
6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
   * s4 t& p8 p" i+ I" }7 N2 y) ?' N; F9 S
7.     NVIC_InitTypeDef NVIC_InitStructure;
   
8. 
   ' {  S. F7 _, {' H  `! F3 B
9.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
   0 f8 I! F/ t, U( I. L
10.     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟
    
11.   
    5 F) J% J( \0 P& R8 J" D
12.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用
    
13.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用
    
14. 
    ! x0 \- C6 V0 A/ j$ F
15.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
    4 ~+ b& v. U/ |$ y  }
16.     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
    
17.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    
18.     GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    
19.     //GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL ;
    1 X  A' T/ y% \( y' }* {5 ~
20.     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
    
21.     GPIO_Init(GPIOB,&GPIO_InitStructure);
    & x  R& @; o3 _% `; t% I3 t
22.    
    % y7 t  n+ i6 u( W" S
23.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    " Q: d' P- T" H0 F
24.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
    
25.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    " b  K" U1 J6 W* O! c
26.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断，防止计数溢出而没有相应函数，造成卡死
    
27.     NVIC_Init(&NVIC_InitStructure);
    
28. 
    1 ~4 T) t  d. E! @0 J4 P" y7 V6 O& `5 G
29. 
    & c2 T4 S/ V# ?9 j8 ?  b0 |* k" Y" c
30. 
    
31.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
    5 M8 V/ u0 k. j' F- L
32.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
    
33.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
    6 l- t9 @7 |! v: G0 }) t
34.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    
35.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    
36. 
    $ d3 f1 P3 X: [+ X
37.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
    - i2 q  Z. `6 T' D, d
38.     TIM_ICStructInit(&TIM_ICInitStructure);
    
39.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
    
40.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
    
41.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
    $ Q* O9 H4 {3 B$ W+ Z
42.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
    + _8 {5 b# K" c7 g& r
43.     TIM4->CNT = 0;
    * {8 @6 Z, U: e; C% ~
44.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
    3 @+ R! Z* e' |* V9 w, C
45. }
    
46. </font>

复制代码

编码器返回速度值：
, T9 D& S" h. _3 Z4 r$ L* ^) o& T# x5 y

1. <font face="微软雅黑" size="3">/**************************************************************************
   7 S. ~% R9 R/ g/ L
2. 函数功能：单位时间读取编码器计数
   
3. 入口参数：定时器
   9 l4 I9 L, H' H2 [1 |
4. 返回  值：速度值
   6 w" N) b  f, l( g0 P4 E; j
5. **************************************************************************/
   
6. float Read_Encoder_Speed(uint8_t TIMX)
   2 t5 w; S! D5 a3 A/ D! V
7. {
   
8.     int32_t Encoder_TIM;
   6 |% U5 P9 r8 b# u
9.     float res = 0;
   " e$ ~% K- d; J3 x  v
10.     switch (TIMX)
    
11.     {
    
12.     case 5:
    * B1 _7 `7 P$ I# X
13.         Encoder_TIM = TIM_GetCounter(TIM5);
    * j* C/ Z! J5 e5 m0 G
14.         TIM5->CNT = ENCODER_BASE_COUNT;
    
15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    7 g, {# P3 o% T$ w9 z& b# d1 u
16.         break;
    7 U# ^% q; b; |$ m) x, S
17.     case 4:
    
18.         Encoder_TIM = TIM_GetCounter(TIM4);
    . q' j. q1 z) Y
19.         TIM4->CNT = ENCODER_BASE_COUNT;
    ( n# j6 K2 S; ]
20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    
21.         break;
    
22.     default:
    
23.         Encoder_TIM = 0;
    
24.         res = 0;
    ( I: ~) c6 t1 d$ c
25.     }
    
26.     if(res>2048.0f)
    + F5 E8 g/ \; q9 \& c8 l; i
27.         res-=4096.0f;
    & T; |. S9 w5 W
28.     return res*360.0f/4096.0f;
    
29. }
    * Y& M+ V6 X1 D8 V' d: A8 Z) ^
30. </font>

复制代码

定时从编码器取数，注意，时间不一样，取回的数值也不一样，取决于实际速度以及编码器线数。这里50ms取一次：

1. <font face="微软雅黑" size="3">
   
2. void TIM7_IRQHandler(void)//频率20HZ，用于编码器计数
   
3. {  
   
4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
   : A% o" b) W3 o0 Z6 z- K
5.   {
   5 s/ O0 u# l9 r7 O6 C
6.     speed=Read_Encoder_Speed(4);
   
7.   }
   . }" `, L; Y: S5 o
8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
   ! E0 @4 c2 |1 Y7 q" r) w
9. }</font>

复制代码

2.5 PID控制


PID库函数：

& o5 S, @1 C* b3 ^

1. <font face="微软雅黑" size="3">
   
2. #define N 2        //需要对多少变量进行pid调节
   : F: O' `+ Z7 V* x1 A6 X
3. 
   
4. const float KP[N]={1.3,1.0};//这里只用了比例调节
   5 a" x3 X1 m% \7 Q7 Z* D  I, R: v% w
5. const float KI[N]={0,0};
   3 Q2 i+ m, T( [) n: V3 U
6. const float KD[N]={0,0};
   & x) o( G9 o+ R
7. 
   6 o) q1 L% c! U0 z  N$ U4 ~5 P+ M
8. 
   " T% D3 P9 C" J$ G% p+ V
9. struct _pid{
   
10.   float SetVol;        //定义设定值
    
11.   float ActVol;        //定义实际值
    % h& B* U5 Y3 a
12.   float Err;          //定义误差
    
13.   float  Err_Next;      //定义上一个误差
    
14.   float  Err_Last;      //定义上上一个误差
    . X5 L7 B+ W5 T& g" K& m
15.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
    ( z) q3 V, A% w5 B9 b# E
16.   float integral;      //定义积分值
    
17.   float actuator;      //定义控制器执行变量
    
18. }pid[N];
    7 G% w9 i2 ]0 J( g: q
19. 
    3 Y* m+ ]5 Y: k8 n2 i  g7 f, I
20. void PID_Init(void)
    - d0 R/ c( W+ A4 J3 E' o5 H. L
21. {
    
22.   for(int i=0;i<N;i++)
    5 }1 X* G1 _% p( J
23.   {
    
24.     pid[i].SetVol=0.0;
    3 Y& e' @! |+ i" s3 L/ m8 y1 c
25.     pid[i].ActVol=0.0;
    
26.     pid[i].Err=0.0;
    
27.     pid[i].Err_Next=0.0;
    
28.     pid[i].Err_Last=0.0;
    + e$ w; b) U: G; v; f
29.     pid[i].integral=0.0;
    
30.     pid[i].actuator=0.0;
      @0 A& z+ l0 ]# E# d- u* {
31.     pid[i].Kp=KP[i];
    2 g8 T6 s: S. ?' e, Z* F1 k
32.     pid[i].Ki=KI[i];
    ) [; T/ D$ J. m* e
33.     pid[i].Kd=KD[i];
    
34.   }
    
35. }
    . N8 c. N# T$ `, q3 e7 V
36. 
    2 f) X/ w) q# ^7 o  z  N" F
37. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
    + c$ y' x: j7 z- ]
38. {
    7 F/ w: I) K; f6 f
39.   pid[i].SetVol=set_val;
    + B! W9 u, F7 c' f
40.   pid[i].ActVol=get_val;
    ! @! N& C6 ~% K( Q5 t3 e5 n! [
41.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
    
42.   float IncVol;    //定义增量
    
43.   pid[i].integral+=pid[i].Err;
    : @6 Z; ^  x* s: T3 Z3 P6 `; T
44. //  IncVol=pid[i].Kp*(pid[i].Err-pid[i].Err_Next)+pid[i].Ki*pid[i].Err+pid[i].Kd*(pid[i].Err-2*pid[i].Err_Next+pid[i].Err_Last);
    
45.   pid[i].actuator=pid[i].Kp* pid[i].Err+pid[i].Ki*pid[i].integral+pid[i].Kd*(pid[i].Err-pid[i].Err_Next);
    
46. //  pid[i].actuator=adc_val+IncVol;
    
47.   pid[i].ActVol=pid[i].actuator;
    & m9 l/ q2 V; `& P7 l$ X+ ?
48.   pid[i].Err_Last=pid[i].Err_Next;
    
49.   pid[i].Err_Next=pid[i].Err;
    4 P% H! S" M4 {* a4 ^8 u
50.   
    ( C, ^- X$ Z. j( E6 `+ V# [: H% X. [
51.   return pid[i].actuator;
    : I6 _6 h. P/ W* C0 L- a
52. }</font>

复制代码

主函数中的PID调节：
3 k7 Z4 j) L: v" L. r% f6 h

1. <font face="微软雅黑" size="3">
   
2.     z_get=data[2];
   
3.     x_get=data[0];
   
4.     if(z_get-z_set>0.5||z_get-z_set<-0.5)//电机PID
   
5.     {
   
6.       LED1=0;  //调节时灯亮
   0 s- A2 K" y( O! }9 Q" Y
7.       PID_val_motor=PID_realize(z_set,z_get,0);
   
8.       PID_val_motor=PID_val_motor/10.0;
   
9.       if(PID_val_motor<=0)
   
10.         motor_flag=0;//motor_flag控制电机正反转，PID_val_motor用于改变占空比，范围0~1
    
11.       if(PID_val_motor>0)
    
12.         motor_flag=1;
    ) l  G7 ?& Q! B
13.       PID_val_motor=abs_float(PID_val_motor);
    * p- m0 ?) M/ j0 _9 z. p, `
14.       if(PID_val_motor>2)PID_val_motor=0;//标志太远，让车停止
    # R. C' j3 b4 b- h4 a6 Q
15.       if(PID_val_motor>1&&PID_val_motor<=2)PID_val_motor=1;
    
16.       if(PID_val_motor<0.2)PID_val_motor=0;
    5 ^3 s; p. P: X  Z
17.     }
    # X  U8 d7 X% G9 |) A) K- a
18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
    
19.     {
    + a+ q- S" p# {# t' V8 R' D- U( C
20.       LED1=0;  
    ) ^  E7 w% f7 X% ~1 m8 m9 n8 A
21.       PID_val_servo=PID_realize(x_set,x_get,1);
    
22.       servo_angle=((140-35)/6)*PID_val_servo+35;//线性映射，把PID的值转化为角度35~140的舵机转角
    % g+ p! m' D/ J  Y4 R
23.       if(servo_angle<35)servo_angle=35;
    
24.       if(servo_angle>140)servo_angle=140;
    
25.     }
    
26.     LED1=1;</font>

复制代码

定时器TIM2中断里改变占空比：

1. <font face="微软雅黑" size="3">
   * i. h/ O7 _+ a1 c7 B
2. void TIM2_IRQHandler(void)
   . V0 I2 w; A3 r4 `4 o8 Q; U. |% y& i
3. {  
   
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   # Y( `) i: E' h0 A% m) k
5.   {
   4 o. W& u* j9 T
6.     if(motor_flag==1)//反转
   " q+ H7 K7 S$ q0 S
7.     {
   ) Q  j& O2 j/ D0 O3 Y, V% n
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   
9.       TIM_SetCompare2(TIM8,0);
   , T$ l' X$ D7 N# F
10.     }
    0 {1 \4 y, e& L' l
11.     if(motor_flag==0)//正转
    7 C' p0 h6 R/ A$ b; z# `3 P" m/ R
12.     {
    
13.       TIM_SetCompare1(TIM8,0);
    2 q% \# k- Y$ F
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    , Z" f! ]( D  C+ K5 L1 L3 R) i7 M9 i
15.     }
    6 U) s, G5 C* Z) j3 m$ n6 m
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，根据舵机手册得到占空比与转角的关系，引脚：PA6
    : q( x0 q1 w# E+ _# ~4 z! O& U( p
17.   }
    
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    , _, P" h# z! u& k/ b8 b
20. </font>

复制代码

1 p! }$ Q4 h- _9 O7 v5 y

. [! Z6 t0 |: U/ O

优秀