概述
小车外形：


3 U; a7 K) {5 B, O  v' S
功能简介
4 G$ p8 o/ ]: }+ h8 L利用摄像头识别前车尾部的AprilTag，得到前车位置，传回stm32主控板处理，使两车在行驶时保持恒定距离，实现自动跟车。
1 G5 Q. S0 A0 ^  G

' o8 C& @. o' X) d8 A* l( Y8 i
% T+ X# Y! C* u9 w/ @
openMV4摄像头
1.1 Apriltag识别与串口传输

/ ?+ A; v" J/ `  k6 d. h, c$ A
AprilTag是一个视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志（与二维码相似，但是降低了复杂度以满足实时性要求），可以快速地检测标志，并计算相对位置。


Apriltag示例：
6 z8 D) H. G4 D0 h
通过识别Apriltag，可以得到x,y,z三个方向的距离以及偏移角度。这里只需要三维的距离即可，通过串口传回stm32.

1. <font face="微软雅黑" size="3">
   5 h) W# L& q+ O3 Y+ p# S
2. import sensor, image, time, math,pyb
   2 ]1 N2 k* ^- ~; s4 |. e
3. from pyb import UART
   0 Q0 C  }( P, q8 `
4. 
   ) m# B$ Q# K. ]: A
5. sensor.reset()
   0 |' [  B! Q, I. |
6. sensor.set_pixformat(sensor.RGB565)
   % p! q$ W$ b' g5 ]5 p# S4 b& P
7. sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
   ) O" T" `2 D2 z+ G) z1 r0 n, Q
8. sensor.skip_frames(time = 2000)
   
9. sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
   
10. sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
    
11. clock = time.clock()
    $ r: J0 W5 \, r' n) q$ ?4 M8 l# ^
12. uart = UART(3, 115200)#串口波特率
    
13. 
    
14. f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
    
15. f_y = (2.8 / 2.952) * 120 # find_apriltags defaults to this if not set
    0 h- a4 K1 l5 p, w  ^
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. 
    
19. def degrees(radians):
    5 s. i9 Y! U% D' ^2 y
20.     return (180 * radians) / math.pi
    1 j% e  Q$ m. M2 L( t, Q
21. 
    & Z$ |2 `  p: ~  R2 p5 Y+ b$ I' |
22. while(True):
    
23.     clock.tick()
    6 \  L! e3 ~1 I# h. \# |; Q& 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))
    $ O9 G- c0 k' m" i# P7 v
27.         img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
    
28.         print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
    ; |0 x2 D( K! N$ P/ b# J
29.             #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
    
30.         # Translation units are unknown. Rotation units are in degrees.
    4 g8 ^) ?) |- |/ E9 W$ r5 v
31.        # print("Tx %f, Ty %f, Tz %f" % print_args)
    2 ]3 r- D) V: f- h
32.         uart.write("A%.2f,B%.2f,C%.2f," % print_args+'\r\n')#设置特定格式，以便于stm32分割取得数据
    / R  `) [( Q8 r& ~
33.         #pyb.delay(500)
    * h8 V9 z% F! H7 Z# n! P% N
34.    # print(clock.fps())</font>

复制代码

STM32主控板（型号为F407）
) N* c5 e" Z  G3 ~8 z0 [

4 L* I( f( n( Q6 a; E' ]% M2.1 时钟与中断配置

: W) H8 E( i6 N2 c- T0 ^; Y- s0 m) y" y
2 \9 A+ ?; r3 l2 B- f: K% t附上stm32时钟示意图：
# E) u! z0 |  |% i- t; I7 ~
定时器示意图：
+ }" P% v1 T, s3 ]' I( C1 e/ A4 w
4 s% A; R  \/ }. V" a% R$ `7 S4 a定时器分配：
, J0 X. F0 w$ N; ?( e" x& t  V
/ A) j% ?0 z1 S" t% j9 v4 f2 a6 Q' U所有时钟初始化的函数：（每个函数的详细内容在后面）

1. <font face="微软雅黑" size="3">
   
2. TIM8_PWM_Init(400-1,20-1);  //用于控制电机，168M/20=8.4Mhz的计数频率,重装载值400，所以PWM频率为 8.4M/400=21Khz.
   
3.   TIM3_PWM_Init(200-1,8400-1);//用于控制舵机，50HZ
   % I! V: q8 b# ]% T. x
4.   TIM2_Int_Init(400-1,20-1);//定时中断，21KHZ
   
5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数，20HZ，50ms中断一次
   , J6 ~1 o+ _  t/ C) I
6.   uart_init(115200);  //初始化串口1波特率为115200
   6 I4 @, M; |5 o6 W* q( N: d
7.   Encoder_Init_TIM4();//编码器接口初始化</font>

复制代码

2.2 串口收发与数据处理


串口中断：USART1，USART2
串口初始化函数（以USART1为例）：

1. <font face="微软雅黑" size="3">
   
2. void uart_init(u32 bound){
   9 I  r8 G  {4 ]. |% j
3.    //GPIO端口设置
   
4.   GPIO_InitTypeDef GPIO_InitStructure;
   
5.   USART_InitTypeDef USART_InitStructure;
   
6.   NVIC_InitTypeDef NVIC_InitStructure;
   
7.   
   
8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
   
9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
   ) t3 X& U' F( F8 e/ `
10. 
    ! k  x; g5 V' R  o
11.   //串口1对应引脚复用映射
    
12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
    
13.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
    7 Q6 i7 e, m, C: ~
14.   
    
15.   //USART1端口配置
    % d: q) }( b) y, u, j
16.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
    " T4 e9 I% s& v/ \, `/ Y9 j
17.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    8 G1 s) {7 Y& P, ~6 m
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; //上拉
    
21.   GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9，PA10
    : p2 T, W; @5 r6 _% }
22. 
    0 L( ^; f; c1 G) T- [$ _
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;//一个停止位
    % x5 C& x  G3 w" e: N  p& @; M; U
27.   USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
    & q$ `; B9 h" }, x' C$ V' P, v) t
28.   USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
    
29.   USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;  //收发模式
    
30.   USART_Init(USART1, &USART_InitStructure); //初始化串口1
    , t- o/ E$ ~6 j" U3 H
31.   
    
32.   USART_Cmd(USART1, ENABLE);  //使能串口1
    
33.   
    
34.   USART_ClearFlag(USART1, USART_FLAG_TC);
    5 R5 [+ n8 u" L" J
35.   
    
36. #if EN_USART1_RX  
    ' @; |+ |3 U7 y9 [+ V9 ?
37.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
    4 y/ [6 [. ]# X' U0 q
38. 
    
39.   //Usart1 NVIC 配置
    
40.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
    
41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
    
42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
      H% W' x1 z2 J! ~
43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
    3 }) m! Q8 i2 i& d5 a! \
44.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、
      v; l/ E- a* p0 _: u' w, k: o
45. 
    6 `; U- z" T' E2 e3 c$ C
46. #endif
    5 H3 ]2 R9 P+ z8 ~7 x6 s
47.   
    ! S! q( T& H: m0 t6 c. p5 q
48. }</font>

复制代码

串口中断处理函数：

1. <font face="微软雅黑" size="3">
   
2. void USART1_IRQHandler(void)                  //串口1中断服务程序
   ( W" R& \1 _8 p  w
3. {
   " u# Q& o2 d5 n2 [
4.   u8 Res;
   7 p# v; C# @2 A! d2 X2 [
5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
   
6.   OSIntEnter();   
   0 a- ]4 P" m& H- K7 ?- @/ [! g
7. #endif
   , a1 u8 j$ E8 U  T) z: G) b* |
8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
   / D  P8 N4 y2 P/ M8 Y
9.   {
   . N4 L/ c! b' U8 x, o+ F
10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
    $ v4 v4 c8 J0 ^* C) n
11.    
    ) P- e9 c+ j$ {8 u" M- g4 B
12.     if((USART_RX_STA&0x8000)==0)//接收未完成
    
13.     {
    
14.       if(USART_RX_STA&0x4000)//接收到了0x0d
    
15.       {
    
16.         if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
    
17.         else USART_RX_STA|=0x8000;  //接收完成了
    
18.       }
    
19.       else //还没收到0X0D
    
20.       {  
    
21.         if(Res==0x0d)USART_RX_STA|=0x4000;
    
22.         else
    " G; }/ e3 w* h) c
23.         {
    
24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
    
25.           USART_RX_STA++;
    1 F9 _7 a+ o% T. g
26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
    
27.         }     
    5 N, O/ _! \7 \0 J
28.       }
    
29.     }        
    
30.   }
    $ D4 r- J% h8 c8 p1 Z7 E  n
31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
    5 ^7 {1 J1 V6 x. ^' y! ]6 f7 [
32.   OSIntExit();                        
    
33. #endif
    & H6 V; I$ e4 M) L3 Q: M: |
34. }
    
35. #endif  </font>

复制代码

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

1. <font face="微软雅黑" size="3">/*涉及到的全局变量
   9 Y( ^! B/ M: ]8 x
2. float data[3];//x,y,z方向的距离，浮点数形式
   
3. unsigned char data_string[3][7];//x,y,z方向的距离，字符串形式
   
4. */
   6 V# C4 J+ Y9 d: t( `  u6 t, s
5. if(USART_RX_STA&0x8000)
   , R8 d& W0 H' p7 V8 F/ p+ S: p
6.     {  
   
7.       //清空字符串
   # V* w: c0 l/ f; l  Z' r
8.       for(i=0;i<2;i++)
   
9.       {
   
10.         for(j=0;j<6;j++)
    
11.         {
    * B# \' Z" t$ r8 Y. O! H/ M% G, y+ c
12.           data_string[i][j]=' ';
    
13.         }
    & ]" v  f( w. D5 x
14.       }
    
15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
    + }4 p/ L* L! @$ Z" V) \
16.       for(t=0,j=0;t<len;t++)
    
17.       {
    
18.         j=0;
    " c3 f0 S' v/ o  M
19.         if(USART_RX_BUF[t]=='A')
    + F1 i0 |0 X4 @; x6 T4 H
20.         {
    , |% i) D5 c1 ^- j' J+ g. {
21.           t++;//去掉首字母
    , D$ a$ d3 _5 d8 I. T
22.           while(USART_RX_BUF[t]!=',')
    , K! @0 ~' V* A
23.           {
    9 ?6 F9 ^( z2 }3 |/ z, J& y4 z: R, t
24.             data_string[0][j]=USART_RX_BUF[t];
    * R8 ~: {' _4 e5 q% Z
25.             t++;
    * {9 E5 _+ H4 o( v
26.             j++;
    
27.           }
    
28.         }
    
29.         if(USART_RX_BUF[t]=='B')
    $ n$ k, t5 H. H& g
30.         {
    1 _) H4 }4 V% W8 y0 L+ E
31.           t++;//去掉首字母
    8 M/ ^2 G% w5 z7 K0 i
32.           while(USART_RX_BUF[t]!=',')
    7 i. ?; f, \: P! i$ y* R/ W
33.           {
    
34.             data_string[1][j]=USART_RX_BUF[t];
    ; \5 U% D! G3 D2 U+ T
35.             t++;
    0 p& ]0 i4 z/ h* \1 V
36.             j++;
    
37.           }
    7 N; F# J, t6 S- {8 r0 A& H! |
38.         }
    
39.         if(USART_RX_BUF[t]=='C')
    " ]0 ~8 p4 Z, t- ?
40.         {
    
41.           t++;//去掉首字母
    . N( Z. @0 x7 g( `( e; I4 g
42.           while(USART_RX_BUF[t]!=',')
    
43.           {
    7 C8 f2 z1 @- s. M& E( x. w
44.             data_string[2][j]=USART_RX_BUF[t];
    , t; ^8 o7 d( s/ ~% p
45.             t++;
    3 p" R& w# w, p
46.             j++;
    4 Y% `$ W2 S  W6 ~' ]9 S6 W  _* X
47.           }
    
48.         }
    " D6 \3 n1 z+ }1 v" ?0 y. w
49.       }
    
50.       //把字符串转化为浮点数
    
51.       data[0]=myatof(data_string[0])/100.0;//x
    
52.       data[1]=myatof(data_string[1])/100.0;//y
    6 H# S% @- L4 ^
53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z，输入是负数，转换为正
    " o5 B; q7 o. L/ g* Z2 T
54.         //USART2发送数据
    6 L. Q/ C' P) l* ]" e1 s
55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
    
56.         
    8 q6 \; n/ F/ [9 w
57.       //LCD更新显示
    . g, `* w1 K! a4 r
58.       //显示xyz
    
59. //        CLEAR(10);
    # M3 x# {' L  `, `8 n+ J
60. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);
    
61. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,data_string[0]);
      x; P: t7 X2 m$ z! v
62. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,clear);
    7 K& P! b% _- e1 M3 ~  q
63. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
    
64. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
    
65. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);
    , E$ r( |3 |/ ?+ U! @' i2 u: D$ F
66.         
    
67.         USART_RX_STA=0;//清除标志位
    
68.     }
    3 M0 I6 E$ z3 U  E; R: ]  s/ j
69. }
    
70. </font>

复制代码

字符串转化为两位小数浮点数（用于后续PID控制）:
  m. m2 W, l: z$ B9 L4 D# F

1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数，返回的是乘100后的int值，因float返回有错误
   
2. {
   / k& I2 o5 a( B( t$ `* Z) \
3.   int flag = 1;//表示正数
   
4.   int res =0;
   / M: D+ l! B+ K1 _" g6 z- T1 w3 d' a
5.   u8 i=1; //小数点后两位
   
6.   while(*str != '\0')
     i* g2 z4 [* `
7.     {
   
8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
   
9.       {
   
10.         str++;
    
11.         continue;
    
12.       }
    ( [3 E7 Y2 [9 e4 B. f: N
13.       if(*(str-1) == '-')
    
14.       {
    * y3 n" i3 k+ [' f/ S1 m3 L
15.         flag=-1;//表示是一个负数
    
16.       }
    
17. 
    " w% p: d. v  S' h! X/ ]2 [
18.     while(*str >= '0' && *str <= '9')
    + k$ Z' [2 [5 c7 G
19.     {
    
20.       res = res *10 + (*str - '0');
    5 {$ _3 n; `' d% V0 c  f. [
21.       str++;
    4 O7 L2 D# ^# z" j+ l; }. t$ f1 C+ z
22.     }
    
23.     if(*str == '.')
    1 W3 ?! k' i" Z
24.     {
    8 \: D1 [+ j; m" r4 A
25.       str++;
    
26.       res = res *10 + (*str - '0');
    ) v. f* p2 d5 U1 P; Z+ H
27.       str++;
    
28.       res = res *10 + (*str - '0');//保留两位，故加两次
    ! \# c/ M- A) M
29.       return res*flag;
    : s7 a7 x0 ~$ z2 |6 B# p( l$ ?
30.     }
      c9 `' \. _3 h* C. p5 H
31.     }
    
32. }
    
33. 
    
34. 
      ]' m; l2 G7 f
35. 2.3 LCD显示模块
    % Q) a' p3 }" q* o
36. 
    
37. LCD模块用于调试时观察数据，调试完成可以删去，因为显示屏很耗时，使处理速度变慢
    2 B0 p0 E$ H8 E6 i
38. 驱动函数总览：
    
39. 
    $ D6 H; d/ }9 G$ q% u6 s. R8 z
40. 
    
41. 
    
42. void LCD_GPIO_Init(void);
    . J) p* z- L$ c: z& B: e
43. void Lcd_WriteIndex(u8 Index);
    
44. void Lcd_WriteData(u8 Data);
    # r- g5 F' B0 u( u6 Z' u* Y; ~1 D
45. void Lcd_WriteReg(u8 Index,u8 Data);
    + [" i# o" z0 ?. ]  A3 c
46. u16 Lcd_ReadReg(u8 LCD_Reg);
    4 `3 F- J& E1 z+ Y5 R+ ?" K2 y
47. void Lcd_Reset(void);
    ) t" j  z; e3 }: a% V
48. void Lcd_Init(void);
    
49. void Lcd_Clear(u16 Color);
    3 L2 o' z. F- M& {' U4 H# M4 u) b
50. void Lcd_SetXY(u16 x,u16 y);
    
51. void Gui_DrawPoint(u16 x,u16 y,u16 Data);
    ( M  p. {" Z- ]! t
52. unsigned int Lcd_ReadPoint(u16 x,u16 y);
    
53. void Lcd_SetRegion(u16 x_start,u16 y_start,u16 x_end,u16 y_end);
    & o- f5 O' Z: B2 T' |' x; W& m
54. void LCD_WriteData_16Bit(u16 Data);</font>

复制代码

, B& F; l6 p/ W
+ m/ ~" A3 e: |! g  yTFT屏幕初始化：

7 I+ l& [' B9 W1 j9 }

1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
   # ^) k& G- J8 F: Z2 I! |0 h
2. {
   ( U7 [( g/ N+ W5 I; E6 r
3.   Lcd_Clear(WHITE);
   
4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
   , k1 L0 B) y. H
5.   delay_ms(1000);
   
6.   Lcd_Clear(WHITE);
   , @# s; z! K! B4 _
7.   Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
   
8.   Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:");
   
9.   Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:");
   
10.   Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");
    
11. }</font>

复制代码

字符串显示函数;

1. <font face="微软雅黑" size="3">
   3 m& k; O, F- _8 I# V7 N& t
2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
   
3. {
   
4.   unsigned char i,j;
   
5.   unsigned short k,x0;
   : W! j) d, f# u0 ~
6.   x0=x;
   - p% B! y. j& j# H( l& M+ H
7. 
   
8.   while(*s)
   
9.   {  
   
10.     if((*s) < 128)
    
11.     {
    
12.       k=*s;
    3 a& o/ Y- M% \3 b; l4 P& ~2 `# K
13.       if (k==13)
    
14.       {
    
15.         x=x0;
    
16.         y+=16;
    
17.       }
    
18.       else
    
19.       {
    & Z* ]5 x+ C* n" b
20.         if (k>32) k-=32; else k=0;
    ( b" x3 G+ j3 t6 O9 S+ n9 ^
21.   
    
22.           for(i=0;i<16;i++)
    
23.         for(j=0;j<8;j++)
    
24.           {
    ' u' P( q2 v. J
25.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    4 e. ]! L7 @7 n- p3 p4 t' {
26.             else
    ' B/ j( p" ~+ x
27.             {
    
28.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
29.             }
    
30.           }
    ( @, J( A' E* z4 {& Q* ]
31.         x+=8;
    ! I6 s; S+ v9 y+ c& j1 [
32.       }
    # I- j& g, x1 |  R! |7 B
33.       s++;
    - @4 u/ V6 ~* o6 R* f( u1 h
34.     }
    
35.       
    
36.     else
    " m$ v0 i- U/ }' e+ P9 f
37.     {
    ( F9 g$ c% N+ E) g! L9 l$ K0 P
38.    
    9 @7 O3 x& E( ^& G4 d
39. 
    3 R% f; I, z6 m& Q9 L$ A- A
40.       for (k=0;k<hz16_num;k++)
    # N* ~6 o# F" h+ r: c0 J2 q
41.       {
    
42.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
    
43.         {
    0 D. ~; W' M4 d+ \
44.             for(i=0;i<16;i++)
    
45.             {
    
46.             for(j=0;j<8;j++)
    6 j8 N/ q. V3 ]
47.               {
    
48.                   if(hz16[k].Msk[i*2]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
49.                 else {
    
50.                   if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    2 o. O2 g# D4 g% {2 V& b  R; C9 Z
51.                 }
    
52.               }
    
53.             for(j=0;j<8;j++)
    
54.               {
    
55.                   if(hz16[k].Msk[i*2+1]&(0x80>>j))  Gui_DrawPoint(x+j+8,y+i,fc);
    ' A# ]1 N6 j, j( v6 l/ [
56.                 else
    
57.                 {
    + k* H0 h: Q: `
58.                   if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);
    
59.                 }
    
60.               }
    
61.             }
    1 z; T9 j7 }, z  C8 j
62.         }
    
63.         }
    
64.       s+=2;x+=16;
    2 M4 a3 r! b% y4 L7 G' e
65.     }
    
66.    
    4 l9 q; l8 H" k' ~5 |, ]% I
67.   }
    % P% ^5 f7 F% N2 h  e
68. }
    " h9 b# N+ v3 o' U1 G/ A& U
69. </font>

复制代码

2.4 电机、舵机与编码器
" S  s5 O& I6 Q4 Y

/ ?3 {- ?1 j1 P; w$ t定时中断：TIM2，用于修改电机和舵机的PWM占空比
初始化函数：
: I, E" a) C6 y/ T9 A# ]/ T/ p0 L( A

1. <font face="微软雅黑" size="3">
   
2. //通用定时器2中断初始化
   + |+ D) c" e; ]" x
3. //arr：自动重装值。
   ; Z5 F, ]. r8 {" x1 H' c5 y# K  f3 m
4. //psc：时钟预分频数
   $ V& I# n" ^0 u/ b
5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
     h, c- \7 z9 l) F" I
6. //Ft=定时器工作频率,单位:Mhz
   ) j! E, p8 m0 |6 u# {5 X/ F$ d; X) Y
7. //这里使用的是定时器2!
   
8. void TIM2_Int_Init(u16 arr,u16 psc)
     `8 y8 \7 ]9 x* A5 |
9. {
   7 c. @1 s& m( T
10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    
11.   NVIC_InitTypeDef NVIC_InitStructure;
    
12.   
    
13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
    6 `. n, V" Z1 }5 w
14.   
    
15.   TIM_TimeBaseInitStructure.TIM_Period = arr;   //自动重装载值
    
16.   TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    
17.   TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    2 {0 s' r" B) Z* D9 q
18.   TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
19.   
    
20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
    
21.   
    
22.   TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断
    
23.   TIM_Cmd(TIM2,ENABLE); //使能定时器2
    . a* S' o# `9 b( q: \
24.   
    
25.   NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器2中断
    , _/ O# G) ?9 C9 [0 A1 P
26.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级1
    
27.   NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
    7 N; S, ]6 x/ @4 K7 \0 K) T* W
28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    
29.   NVIC_Init(&NVIC_InitStructure);
    
30. 
    
31. }</font>

复制代码

TIM2中断处理函数：

1. <font face="微软雅黑" size="3">
   + }" I, u' M  B" {
2. void TIM2_IRQHandler(void)
   
3. {  
   ; B- o9 Y- E! l: C
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   
6.     if(motor_flag==1)//反转
   
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   : c/ V: l8 M3 [) A3 }
9.       TIM_SetCompare2(TIM8,0);
   1 H; s. X& _* _% \9 O: G- q$ G
10.     }
    1 B, D( M) Q3 X7 I. W/ w8 D0 H- d" s
11.     if(motor_flag==0)//正转
    " k0 \- [3 C+ o
12.     {
    
13.       TIM_SetCompare1(TIM8,0);
    
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    ' X( n8 T9 ?3 ]( \, y
15.     }
    
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，引脚：PA6
    % j6 J, x6 g6 s* i
17.   }
    0 y# E. l* O7 q& f" G; x
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    
20. </font>

复制代码

PWM输出：TIM3（舵机），TIM8（电机）
! j9 Y+ S' m- `) I. a8 v8 }5 J初始化函数（以TIM8为例）：
0 Y- _/ k5 `$ m7 R, `
$ Q0 t; Q" O- G

1. <font face="微软雅黑" size="3">
   
2. void TIM8_PWM_Init(u32 arr,u32 psc)
   $ _& k' R4 X- [8 ~; i
3. {               
   : y$ t. B  _3 |. o! J  P1 [
4.   //此部分需手动修改IO口设置
   $ F& p7 q$ I/ V
5.   
   
6.   GPIO_InitTypeDef GPIO_InitStructure;
   
7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   
8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
   * O+ O* V( V- T- }8 g
9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
   
10.   
    
11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
    
12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
    3 j6 q; y" \' e# h3 Z9 X: U
13.   
    & ?! o. M4 B# V3 s4 `  J
14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
    
15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    $ E& J  u& u8 D. d' {; s+ |7 U9 }
16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
    : B! d/ e. P) m  v
17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    * B; Z6 x2 ^6 }
18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
    
19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7
    ; l0 N. _: @8 @  _3 E
20. 
    & f) g- N% O3 ~9 D
21.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    ; q% e6 A! @3 z
22.   GPIO_Init(GPIOB,&GPIO_InitStructure);
    2 U0 P! D2 C/ \/ M) W" t1 V8 w
23.   
    0 U( c9 X" a! I' y
24.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
    
25.   GPIO_Init(GPIOC,&GPIO_InitStructure);
    
26.   
    
27.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
    
28.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1
    $ E+ b3 C, e' D) B) h- [) m" q
29.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
    
30.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13复用为定时器1
    
31.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14复用为定时器1
    * y; e7 r* l/ @( u1 A. e) S0 ?  a: Z% {
32.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15复用为定时器1
    3 I. F9 b& }' D2 s
33. 
    
34.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
    
35.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
36.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
    5 v3 `* m6 U3 P' X  l( N9 I
37.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    8 g/ {' R" R# k4 r& B- a" s. O
38.   
    ! m5 j" f- ?6 Z) a
39.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
    3 }0 s) Y4 D9 |4 G6 I3 i5 r- F
40.   
    5 b0 V+ G7 z" e( y; x$ o$ O  x& _2 b
41.   //初始化PWM模式   
    . o+ N+ W+ N# {5 o: x8 T: ]. ~
42.   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    
43.   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    2 j% y. @+ G' f& `1 ]7 B/ d8 }: [
44.   TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    , I6 {/ t. G% B% k0 x4 A
45.   TIM_OCInitStructure.TIM_Pulse = 0;
    
46.   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    
47.   TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
    ) |8 @3 _! [3 F
48.   TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
    
49.   TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
    
50. 
    
51.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    9 [; ?" G' A5 [' ^% f' x+ R# ^& ]) t
52.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    
53.   TIM_OC3Init(TIM8, &TIM_OCInitStructure);
    
54. 
    
55.   TIM_OC1PreloadConfig(TIM8,TIM_OCPreload_Enable);
    : F3 ]' P$ Y  r" Y
56.   TIM_OC2PreloadConfig(TIM8,TIM_OCPreload_Enable);
    : l5 k" W* Z0 S2 g3 }
57.   TIM_OC3PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
58. 
    ; |9 ?/ j5 n1 Q8 B" ^" Q
59.   TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
    9 G' G9 f  ^4 C$ e2 Y
60.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
    
61.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
    ! s4 D6 l1 d( v9 ?* E' f( V3 c8 x
62.   TIM_BDTRInitStructure.TIM_DeadTime = 0;
    
63.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
    
64.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;
    ) |# i' e6 u, z# D5 Q
65.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
    
66.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);
    
67. 
    
68.   TIM_Cmd(TIM8,ENABLE);
    / @( B  `8 P: `2 A- B- I: S
69.   TIM_CCPreloadControl(TIM8,ENABLE);
    
70.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
    
71.                      
    
72. }</font>

复制代码

编码器初始化函数：
6 M( e" t; i# V5 ~- \6 W2 g" |  @

1. <font face="微软雅黑" size="3">
   ) g, `5 L! F% a) R: J
2. void Encoder_Init_TIM4(void)
   9 x6 m0 f2 p3 k7 }) Y1 ^" B
3. {
   
4.     GPIO_InitTypeDef         GPIO_InitStructure;
   * l, L# G- l5 ^7 x
5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   & V" m9 {, U! q. U
6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
   2 E- U* |, I! R, f! g
7.     NVIC_InitTypeDef NVIC_InitStructure;
   
8. 
   ( v1 o; N- A+ e; Y  d3 [3 N+ ]
9.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
   
10.     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟
    
11.   
    
12.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用
    
13.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用
    & [3 J6 v5 C$ N
14. 
    
15.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
    
16.     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
    $ c4 Y! S1 X- n
17.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    ( G2 C3 T% [9 }: k7 ^- P" k
18.     GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    
19.     //GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL ;
    
20.     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
    
21.     GPIO_Init(GPIOB,&GPIO_InitStructure);
    
22.    
    " D7 S5 m- R+ T  U1 L
23.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    1 i* W9 |- X6 C5 e4 g2 n0 b- }8 K; l* G2 y
24.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
    
25.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    
26.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断，防止计数溢出而没有相应函数，造成卡死
    
27.     NVIC_Init(&NVIC_InitStructure);
    
28. 
    
29. 
    
30. 
    1 ~- A1 F; A4 ~( U0 T( e4 z( H
31.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
    # R4 M9 L- g1 ~* D5 g4 x5 Z2 @+ I, B
32.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
    
33.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
    8 x1 |' T9 x! f; s. r
34.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    
35.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    " ?7 R$ K- ~4 z. q/ |! ^4 i3 ]
36. 
    ! ^) o5 H0 `6 A
37.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
    
38.     TIM_ICStructInit(&TIM_ICInitStructure);
    
39.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
    ; W) ]( E; h0 {
40.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
    
41.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
    
42.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
    - G9 e* Y: H7 j+ ]: a1 g
43.     TIM4->CNT = 0;
    / ^; a. H$ f* u0 a' V
44.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
    ! ?4 Y7 c9 ]4 w
45. }
    
46. </font>

复制代码

编码器返回速度值：

1. <font face="微软雅黑" size="3">/**************************************************************************
   3 m: o) \- _0 A
2. 函数功能：单位时间读取编码器计数
   & l; ~5 w, o; A. g
3. 入口参数：定时器
   1 c: m+ b4 R  i2 `% Q
4. 返回  值：速度值
   6 L" k$ }1 V+ g9 _7 r: e' h, H6 K
5. **************************************************************************/
   & Y$ ~, X  j% P8 G7 z
6. float Read_Encoder_Speed(uint8_t TIMX)
   8 d5 B, J  @3 m. n' h3 u; E
7. {
   
8.     int32_t Encoder_TIM;
   
9.     float res = 0;
   1 N! @0 Z: ~, S( v. ^2 E
10.     switch (TIMX)
    . `% k1 N  c  z/ X' Q3 Y
11.     {
    
12.     case 5:
    / a/ j$ u" U& T6 `) c0 j0 w: D0 U( F
13.         Encoder_TIM = TIM_GetCounter(TIM5);
    . C/ r+ _% g: Z* S
14.         TIM5->CNT = ENCODER_BASE_COUNT;
    
15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    # ^2 S, F9 j3 {" N
16.         break;
    8 D  ^# e0 j5 l4 T
17.     case 4:
    8 {: h8 z5 n* e* d% w  e1 o
18.         Encoder_TIM = TIM_GetCounter(TIM4);
    1 j  E5 H8 y+ r5 H% M- ]/ }
19.         TIM4->CNT = ENCODER_BASE_COUNT;
    / F9 i8 t/ K0 t) o2 b! q9 G! }
20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    ' c3 R* |9 [& g
21.         break;
    % i3 z- }! V8 ~
22.     default:
    
23.         Encoder_TIM = 0;
    / t% @) Y1 j$ X$ ^+ ?
24.         res = 0;
    
25.     }
    
26.     if(res>2048.0f)
    
27.         res-=4096.0f;
    ; U+ g& q6 Q, i& D" E* l  ~
28.     return res*360.0f/4096.0f;
      U/ G1 z# V' p: L; T$ ^- q9 m
29. }
    ; G* I) o1 T* D+ L) d5 j
30. </font>

复制代码

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

1. <font face="微软雅黑" size="3">
   
2. void TIM7_IRQHandler(void)//频率20HZ，用于编码器计数
   9 }, {8 u: M) {+ s
3. {  
     R3 D0 Z3 U+ \( d
4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
   : k- b* o3 a6 e# h
5.   {
   
6.     speed=Read_Encoder_Speed(4);
   
7.   }
   ( M% h) T6 ?1 t9 u* g
8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
   3 u1 S3 m9 G5 x+ Y/ X( K# B, `8 c: [
9. }</font>

复制代码

2.5 PID控制
0 M5 T3 F! \; {0 E

PID库函数：
. c# P2 M  m; d5 u6 c% n
! I5 F5 i% H- l" M6 Q$ e

1. <font face="微软雅黑" size="3">
   
2. #define N 2        //需要对多少变量进行pid调节
   8 S. k0 b* x1 X! X) w
3. 
   7 D# e0 V, u% T) ^  _; l
4. const float KP[N]={1.3,1.0};//这里只用了比例调节
   
5. const float KI[N]={0,0};
   
6. const float KD[N]={0,0};
   , ^, H5 G* I4 Q' D9 d
7. 
   " j/ t4 J5 s" K- W) B
8. 
   8 s' |6 q1 C" \) o# e* t* M
9. struct _pid{
   . O, J" ^+ [% j" j
10.   float SetVol;        //定义设定值
    ! `3 Q  N. g: l' p1 }
11.   float ActVol;        //定义实际值
    ( x& Q* P: |4 v2 k! Y
12.   float Err;          //定义误差
    1 \0 v* c, A, z
13.   float  Err_Next;      //定义上一个误差
    * ~# C$ ?# _8 c; t4 y/ k
14.   float  Err_Last;      //定义上上一个误差
    1 g/ M5 h  z" a; ]4 \
15.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
    
16.   float integral;      //定义积分值
    1 k8 f( f. }4 A4 p
17.   float actuator;      //定义控制器执行变量
    
18. }pid[N];
    
19. 
    ! S- [, j6 C' V) Q' j
20. void PID_Init(void)
    
21. {
    
22.   for(int i=0;i<N;i++)
    # \& F; j6 `7 [" b
23.   {
    
24.     pid[i].SetVol=0.0;
    
25.     pid[i].ActVol=0.0;
    
26.     pid[i].Err=0.0;
    
27.     pid[i].Err_Next=0.0;
    - M: C" ~0 y$ x& t) K* V
28.     pid[i].Err_Last=0.0;
    1 K5 `7 l- }" L! P% H; H4 p
29.     pid[i].integral=0.0;
    
30.     pid[i].actuator=0.0;
    
31.     pid[i].Kp=KP[i];
    * M4 G' K1 z( r( H
32.     pid[i].Ki=KI[i];
    ; Z3 D" u% J9 U- Q# Q; ~# L" R3 ^
33.     pid[i].Kd=KD[i];
    6 S; A6 W* B4 T8 {
34.   }
    ; C* H6 I2 p5 d  A. ?3 w
35. }
    
36. 
    
37. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
    ' P% [( d2 @/ r
38. {
    8 N1 C0 g5 [9 A& R
39.   pid[i].SetVol=set_val;
    ) }/ i! k6 w* i2 L/ c; ^8 W9 s" C
40.   pid[i].ActVol=get_val;
    
41.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
    ; R- u% P7 n- q( \* i  e
42.   float IncVol;    //定义增量
    # n" d2 a5 C* h# C0 W
43.   pid[i].integral+=pid[i].Err;
    
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);
    1 f3 T6 x# O# n6 o1 E
46. //  pid[i].actuator=adc_val+IncVol;
    
47.   pid[i].ActVol=pid[i].actuator;
    
48.   pid[i].Err_Last=pid[i].Err_Next;
    6 [; C' i) K  n7 u
49.   pid[i].Err_Next=pid[i].Err;
    
50.   
    
51.   return pid[i].actuator;
    8 i6 r- g0 W( _- R8 K
52. }</font>

复制代码

主函数中的PID调节：

1. <font face="微软雅黑" size="3">
   . A) `4 t9 K) X( s4 g- D* j
2.     z_get=data[2];
   ) p% U: Z- }7 f. T* V
3.     x_get=data[0];
   ! R+ x7 w. V- S# D
4.     if(z_get-z_set>0.5||z_get-z_set<-0.5)//电机PID
   * ?: |4 y: T- J/ Q+ }
5.     {
   
6.       LED1=0;  //调节时灯亮
   $ H3 A6 L+ E! ?
7.       PID_val_motor=PID_realize(z_set,z_get,0);
   5 J4 Z& d% f8 X8 `) U4 g7 S: x) C
8.       PID_val_motor=PID_val_motor/10.0;
   6 ]$ A: {. [. h' _$ ]3 W. d
9.       if(PID_val_motor<=0)
   - d4 `/ i3 U, z" F" z
10.         motor_flag=0;//motor_flag控制电机正反转，PID_val_motor用于改变占空比，范围0~1
    ' L- }# {. Z; L" I7 j- R
11.       if(PID_val_motor>0)
    # h' l* V* Q( ?+ r7 e% t
12.         motor_flag=1;
    . d" p# H5 y: Z
13.       PID_val_motor=abs_float(PID_val_motor);
    
14.       if(PID_val_motor>2)PID_val_motor=0;//标志太远，让车停止
    
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;
    
17.     }
    6 |, h  C7 {8 \$ A; c- ~7 Z" W
18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
    4 ~6 a% H( W# B' `3 {
19.     {
    
20.       LED1=0;  
    ; ~) a1 M/ A( [9 }5 o( u7 _8 P
21.       PID_val_servo=PID_realize(x_set,x_get,1);
    . C6 k6 f* s7 {! Q; C
22.       servo_angle=((140-35)/6)*PID_val_servo+35;//线性映射，把PID的值转化为角度35~140的舵机转角
    
23.       if(servo_angle<35)servo_angle=35;
    
24.       if(servo_angle>140)servo_angle=140;
    3 y4 f  E7 Q6 o  s# h
25.     }
    
26.     LED1=1;</font>

复制代码

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

1. <font face="微软雅黑" size="3">
   0 a# Y) r- W% J& c
2. void TIM2_IRQHandler(void)
   
3. {  
   
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   
6.     if(motor_flag==1)//反转
   
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   
9.       TIM_SetCompare2(TIM8,0);
   / G* e5 ~' `) ]8 x: F- {
10.     }
    9 e) V  R5 G: o% I1 w% W
11.     if(motor_flag==0)//正转
    
12.     {
    5 n, Q" o& a, s! H7 s7 k9 j8 t
13.       TIM_SetCompare1(TIM8,0);
    
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    
15.     }
    
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，根据舵机手册得到占空比与转角的关系，引脚：PA6
    0 _9 I7 i3 |8 o4 T# Y% Q
17.   }
    2 ~5 N# k# I: w. o  G
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    
20. </font>

复制代码

0 m7 ^' R1 v/ i% {( ?

优秀
1 U: f2 x" ?8 Z$ A; i