概述
# e; I& P9 Z% `; A: i小车外形：

2 n* u# Y& }' m
+ f5 `* @2 E! z1 H" ~& i
) [$ ^# H- M+ s4 X3 n( l& w功能简介
. e/ \1 y$ n' N" u# O# x, Y- F# a利用摄像头识别前车尾部的AprilTag，得到前车位置，传回stm32主控板处理，使两车在行驶时保持恒定距离，实现自动跟车。
5 `. Y( g/ B+ D7 ^7 v3 Y* W
4 k  M! y6 ]- O& y1 Z4 B, ], {  f

" R0 ]% o: W8 v; K9 u8 I
openMV4摄像头
1.1 Apriltag识别与串口传输


5 N* [. N' g" e" o' xAprilTag是一个视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志（与二维码相似，但是降低了复杂度以满足实时性要求），可以快速地检测标志，并计算相对位置。
) `" ]) G9 L0 p1 o
1 R3 ^0 b$ Y$ |) N/ \5 Q
Apriltag示例：

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

1. <font face="微软雅黑" size="3">
   ( U1 {: Y* t& C/ A) }" r
2. import sensor, image, time, math,pyb
   , C2 y2 ?; M8 l! ~; l7 L
3. from pyb import UART
   
4. 
     ?: A6 W" c! ?" L! C
5. sensor.reset()
   
6. sensor.set_pixformat(sensor.RGB565)
   # ]7 `+ J% ^- \+ t; ], K2 `  l" M
7. sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
   $ |% Z# y$ b3 m
8. sensor.skip_frames(time = 2000)
   
9. sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
   & I; C2 M' ]3 U/ ]7 D
10. sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
    / d  R( V8 v( B/ m& C7 |& u
11. clock = time.clock()
    
12. uart = UART(3, 115200)#串口波特率
    
13. 
      ~. j' t& |: u4 O4 K+ k! f1 d: p
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
    
16. c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
    - ^; L, Q/ U7 Z7 Q: u
17. c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
    7 m0 G6 E+ A6 r* `2 m, \- m9 t
18. 
    
19. def degrees(radians):
    : A9 [, v- i5 M1 H
20.     return (180 * radians) / math.pi
    
21. 
    1 u* S( i: a6 t- Y
22. while(True):
    
23.     clock.tick()
    . g0 R8 ~+ u1 ]
24.     img = sensor.snapshot()
    3 j& S/ `1 n5 c7 e! t
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))
    
28.         print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
    6 S0 Z! t% ?8 h8 O1 b
29.             #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
    * F* W' a6 o5 c* g
30.         # Translation units are unknown. Rotation units are in degrees.
    1 S- r& o  b# p( w; B3 h
31.        # print("Tx %f, Ty %f, Tz %f" % print_args)
    
32.         uart.write("A%.2f,B%.2f,C%.2f," % print_args+'\r\n')#设置特定格式，以便于stm32分割取得数据
    + u/ g. r. u3 z! Y% o2 M# J7 o
33.         #pyb.delay(500)
      h1 {' }  K1 ]( V, Y
34.    # print(clock.fps())</font>

复制代码

STM32主控板（型号为F407）

) [% C0 Q8 D) ^' d
2.1 时钟与中断配置

5 C' s3 s; p5 {
附上stm32时钟示意图：
, _+ F, _1 Y2 n8 Y4 V1 l
" V3 |! h5 w( s; p2 F+ S定时器示意图：
/ J) m+ A' s& N& V1 `8 J
定时器分配：
4 I) _) P& E4 d5 ?) P/ U
所有时钟初始化的函数：（每个函数的详细内容在后面）
3 h  e$ `% g# u* j5 l0 G+ J- J/ e

1. <font face="微软雅黑" size="3">
   
2. TIM8_PWM_Init(400-1,20-1);  //用于控制电机，168M/20=8.4Mhz的计数频率,重装载值400，所以PWM频率为 8.4M/400=21Khz.
   8 `+ r  c. ]1 k; N# S
3.   TIM3_PWM_Init(200-1,8400-1);//用于控制舵机，50HZ
   
4.   TIM2_Int_Init(400-1,20-1);//定时中断，21KHZ
   % t8 C8 c+ ?5 f1 h' k
5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数，20HZ，50ms中断一次
   
6.   uart_init(115200);  //初始化串口1波特率为115200
   4 V6 }" n, n0 [+ w! r: H
7.   Encoder_Init_TIM4();//编码器接口初始化</font>

复制代码

2.2 串口收发与数据处理

; y# s1 `/ O, _. j! w
串口中断：USART1，USART2
串口初始化函数（以USART1为例）：

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;
   
7.   
   
8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
   
9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
     u; r4 v1 l3 P) y
10. 
    8 [6 N# F# c4 S
11.   //串口1对应引脚复用映射
    
12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
    
13.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
      Y" T, G. ?5 K$ e, c5 Z
14.   
    
15.   //USART1端口配置
    7 E4 m: n' d1 g2 y' |. ^" S/ E# P
16.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
    
17.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    7 U. j7 \$ B0 S+ L# \9 o0 S5 w
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
    
22. 
    
23.    //USART1 初始化设置
    ) w4 M" H+ G) h4 I7 [! n
24.   USART_InitStructure.USART_BaudRate = bound;//波特率设置
    * D/ M: t  O- \4 ^' E: ]9 ]+ F0 ?
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;//无奇偶校验位
    1 T* Q( Q) E4 U' V, h0 Q' B
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
      Z( Q4 @3 U0 X7 J7 C0 y
31.   
    
32.   USART_Cmd(USART1, ENABLE);  //使能串口1
    
33.   
    
34.   USART_ClearFlag(USART1, USART_FLAG_TC);
    . i+ B& Z  j# [6 S) R
35.   
    
36. #if EN_USART1_RX  
    
37.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
    ' T, z8 }8 O9 Q' ~9 z% {3 b1 u* F
38. 
    * n3 N' I, D+ r! Z! s
39.   //Usart1 NVIC 配置
    
40.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
    * x% J2 j; M8 y& H+ C% N
41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
    
42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
    
43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
    
44.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、
    
45. 
    
46. #endif
      ?8 u9 q* j3 F* {8 ^8 d) s7 y
47.   
    
48. }</font>

复制代码

串口中断处理函数：

1. <font face="微软雅黑" size="3">
   : p; w/ x( D2 n2 f* J
2. void USART1_IRQHandler(void)                  //串口1中断服务程序
   % @( z7 ~) i' H
3. {
   
4.   u8 Res;
   
5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
   
6.   OSIntEnter();   
   ; U4 {. [: c% [) Z
7. #endif
   . ~0 L7 A$ X' R
8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
   
9.   {
   
10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
    9 }4 W% H: s: [! i. d" [
11.    
    
12.     if((USART_RX_STA&0x8000)==0)//接收未完成
    ! t7 I2 R/ R' b* {
13.     {
    
14.       if(USART_RX_STA&0x4000)//接收到了0x0d
    
15.       {
    . b. ^) I* X5 I# x& u  A% R4 n
16.         if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
    
17.         else USART_RX_STA|=0x8000;  //接收完成了
    , b4 Q, X; S: d% r! K6 _3 G
18.       }
    ; V# W" |# W! \% g% K" A) Z- S
19.       else //还没收到0X0D
    
20.       {  
    
21.         if(Res==0x0d)USART_RX_STA|=0x4000;
    
22.         else
    + d7 }* N# P6 S# w! @
23.         {
    
24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
    
25.           USART_RX_STA++;
    
26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
    
27.         }     
    5 r  i3 p2 O; m% t4 y/ G
28.       }
    
29.     }        
    4 j( u0 k6 R4 d
30.   }
    
31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
    ( W3 G% a3 U2 x8 i5 A( r
32.   OSIntExit();                        
    
33. #endif
    
34. }
    2 W& S4 N7 ]& O
35. #endif  </font>

复制代码

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

1. <font face="微软雅黑" size="3">/*涉及到的全局变量
   
2. float data[3];//x,y,z方向的距离，浮点数形式
   
3. unsigned char data_string[3][7];//x,y,z方向的距离，字符串形式
   
4. */
   
5. if(USART_RX_STA&0x8000)
   
6.     {  
   
7.       //清空字符串
   
8.       for(i=0;i<2;i++)
   % }  u3 H; \2 y, D
9.       {
   
10.         for(j=0;j<6;j++)
    ! h- g" l) Z; ~( r& E% O/ Z8 T+ {+ W
11.         {
    
12.           data_string[i][j]=' ';
    5 ^3 I# X2 R9 x" P( x
13.         }
    * r( `, V- n+ R5 P- c$ ^1 E
14.       }
    
15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
    8 `! q, |$ D7 v$ `+ m+ V  D3 t
16.       for(t=0,j=0;t<len;t++)
    
17.       {
    
18.         j=0;
    
19.         if(USART_RX_BUF[t]=='A')
    , [6 R% h' E! u6 g6 ^
20.         {
    : E" U4 T  E5 r4 e7 R2 u
21.           t++;//去掉首字母
    
22.           while(USART_RX_BUF[t]!=',')
    
23.           {
    . k5 P( V% N1 J4 y- k
24.             data_string[0][j]=USART_RX_BUF[t];
    5 \9 q* E& v) [5 k' \6 v
25.             t++;
    ' e( v$ U) n: c. W
26.             j++;
    % m+ X0 O3 y: B! j; P+ U1 L: J
27.           }
    
28.         }
    ) p- w1 q* x0 a, c3 r
29.         if(USART_RX_BUF[t]=='B')
    
30.         {
    
31.           t++;//去掉首字母
    
32.           while(USART_RX_BUF[t]!=',')
    : f# t4 Y# J7 s
33.           {
    
34.             data_string[1][j]=USART_RX_BUF[t];
    9 _& J$ r6 Q: D* f( K, j- N5 j1 z
35.             t++;
    , F( C! x# n" F7 |( m
36.             j++;
    ' v' x5 X0 g3 `! Q  V2 C
37.           }
    ) A3 D+ j. T3 R$ ]7 Y: B
38.         }
    
39.         if(USART_RX_BUF[t]=='C')
    + _- Y1 Z+ m' o/ H% @# k" w. t# T
40.         {
    
41.           t++;//去掉首字母
    2 x: S" @' \( A8 D/ N
42.           while(USART_RX_BUF[t]!=',')
    9 t) j* P& n$ W. J% _! P
43.           {
    
44.             data_string[2][j]=USART_RX_BUF[t];
    3 M1 A% ]) p0 M3 X4 ]" J
45.             t++;
    2 O& s* P! k6 [% O/ f
46.             j++;
    
47.           }
      U% ]. f9 }& m: _! ^5 L
48.         }
    
49.       }
    2 s1 Q. v$ v4 b* y5 A
50.       //把字符串转化为浮点数
    1 U; i2 f; g4 k! A% l4 D1 j! J) V
51.       data[0]=myatof(data_string[0])/100.0;//x
    
52.       data[1]=myatof(data_string[1])/100.0;//y
    1 N; R9 B0 c( {
53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z，输入是负数，转换为正
    8 {) l" V/ S  ~' _2 r
54.         //USART2发送数据
    
55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
    ' _/ \7 }6 U  o# w1 M( \
56.         
    " J$ y' }& n9 j, m& \
57.       //LCD更新显示
    
58.       //显示xyz
    " |& K6 [" Z) k8 k! [7 i
59. //        CLEAR(10);
    - j: s' [+ G  E) e* _
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);
    
63. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
    $ S$ M9 V% N) r; C" D
64. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
    
65. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);
    ! o+ c9 y9 p! ]" M6 t( n' `
66.         
    . G  T% _4 l" f" {' C( ]# ]1 a& _
67.         USART_RX_STA=0;//清除标志位
    ) R5 i( E9 p$ [( m. U* D
68.     }
    9 i( J% s0 m0 {) D: L* t& e% `+ X
69. }
    
70. </font>

复制代码

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

7 |0 S7 d8 {( z9 M( l

1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数，返回的是乘100后的int值，因float返回有错误
   & t8 a! ]# ?/ V
2. {
   
3.   int flag = 1;//表示正数
   
4.   int res =0;
   . k2 y( L2 C% F3 r; i1 C
5.   u8 i=1; //小数点后两位
   0 W* U! S4 u& X1 R( W/ S/ {
6.   while(*str != '\0')
   
7.     {
   7 T' W3 |. f  T9 E, }" @
8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
   ( q. E4 S* j+ z, M! Y
9.       {
   . a: a2 q# T6 W2 g: Q7 \3 L
10.         str++;
    # M9 h' t, g3 Q
11.         continue;
    & ]3 E$ D! b9 J$ a4 I+ j& x) j) a! r
12.       }
    
13.       if(*(str-1) == '-')
    
14.       {
      a6 ~% r) R; f
15.         flag=-1;//表示是一个负数
    3 O1 }- e% c0 @; i
16.       }
    
17. 
    
18.     while(*str >= '0' && *str <= '9')
    " N4 A( n# \, X2 ]
19.     {
    
20.       res = res *10 + (*str - '0');
    ' M0 [7 F0 v  M  G5 {+ q" t- f
21.       str++;
    , d' q9 p+ c: q# l
22.     }
    
23.     if(*str == '.')
    & T9 F; \" T/ \1 o' G' u: r
24.     {
    1 r6 e8 ?5 Z5 k3 ^
25.       str++;
    
26.       res = res *10 + (*str - '0');
    
27.       str++;
    
28.       res = res *10 + (*str - '0');//保留两位，故加两次
    $ m* k# N. C* L  S. V# s- F: z: p& K
29.       return res*flag;
    * [6 |/ U4 C' _
30.     }
    
31.     }
    
32. }
    % L7 M  P3 G9 w' c: d
33. 
    
34. 
    3 w; z- F6 S3 x+ }/ J
35. 2.3 LCD显示模块
      w* e; h+ a+ R4 p2 J8 q) ~
36. 
    % `: \! C8 U9 t- u# \
37. LCD模块用于调试时观察数据，调试完成可以删去，因为显示屏很耗时，使处理速度变慢
    9 E/ u6 S9 W5 H9 n; C
38. 驱动函数总览：
    6 e; u3 u; M. Z/ c" G7 a
39. 
    
40. 
    1 q5 k! E) b+ I1 X9 l
41. 
    
42. void LCD_GPIO_Init(void);
    - ?$ c4 Y! ?( ]7 }. J7 d
43. void Lcd_WriteIndex(u8 Index);
    
44. void Lcd_WriteData(u8 Data);
    7 {1 k1 G7 v$ N
45. void Lcd_WriteReg(u8 Index,u8 Data);
    
46. u16 Lcd_ReadReg(u8 LCD_Reg);
    
47. void Lcd_Reset(void);
    - B' O) \7 Y% K' |1 x
48. void Lcd_Init(void);
    
49. void Lcd_Clear(u16 Color);
    
50. void Lcd_SetXY(u16 x,u16 y);
    
51. void Gui_DrawPoint(u16 x,u16 y,u16 Data);
    
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);
    
54. void LCD_WriteData_16Bit(u16 Data);</font>

复制代码

; l& ^6 p0 m1 I0 S% M! c
TFT屏幕初始化：
; R! d0 C5 t' ~2 N# g6 k
; R$ e9 T3 }* M2 o& H4 ^

1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
   7 G( F% t$ y+ d/ ~4 ^
2. {
   * z8 X6 B8 ]6 [  a) d0 _
3.   Lcd_Clear(WHITE);
   ) E$ M. _. R" Z4 o5 F* J5 [
4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
   
5.   delay_ms(1000);
   ! ]) }% x/ S4 {5 s
6.   Lcd_Clear(WHITE);
   * d9 R) ~$ ?; x. @2 w
7.   Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
   ! w  |$ m8 W0 S; t
8.   Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:");
   
9.   Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:");
   # J+ ]6 I+ z; N# Z7 O: J
10.   Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");
    & I0 H9 D( m1 S* D/ `
11. }</font>

复制代码

字符串显示函数;

1. <font face="微软雅黑" size="3">
   & d% Q! n) Z  D  Z" U( E) i
2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
   
3. {
   
4.   unsigned char i,j;
   " P1 K# X" i* Z# I
5.   unsigned short k,x0;
   8 f6 `# e7 J0 \* u
6.   x0=x;
   
7. 
   7 y7 d: j7 s0 v2 z. g& M
8.   while(*s)
   
9.   {  
   % \: }: b2 s7 O
10.     if((*s) < 128)
    
11.     {
    
12.       k=*s;
    
13.       if (k==13)
    
14.       {
    
15.         x=x0;
    
16.         y+=16;
    
17.       }
    * q8 s: z" W. d) }" K
18.       else
    ; K* _5 q# M" {/ V
19.       {
    
20.         if (k>32) k-=32; else k=0;
    0 e- _: H* h8 ?  c
21.   
    
22.           for(i=0;i<16;i++)
    
23.         for(j=0;j<8;j++)
    
24.           {
    5 u! q/ N8 X" }
25.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
26.             else
    
27.             {
    + v0 ~% z$ l& J; k6 _% e
28.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
29.             }
    4 K5 N" [  j+ w% ^- e+ I' s. {3 j1 W
30.           }
    . g' b& R/ T" L# C+ c
31.         x+=8;
    2 R7 ]* J' x8 s5 F) w! c6 g
32.       }
    
33.       s++;
    
34.     }
    
35.       
    / K' a. U) C1 z4 J8 Y, h
36.     else
    1 A, w9 y" R* t% V% v  ^1 Y! V. P
37.     {
    ! I! q5 m% i0 I
38.    
    
39. 
    2 j, y4 G+ a7 R* s# f- F- y5 R
40.       for (k=0;k<hz16_num;k++)
    # U" ]% B/ q' j- v6 q, h" U1 S
41.       {
    % _: o. W+ _. P; d
42.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
    - v' x) G# O& e9 {
43.         {
    , E8 a% K# W) l& J+ D
44.             for(i=0;i<16;i++)
    8 g* @6 {. Y( Q
45.             {
    
46.             for(j=0;j<8;j++)
    
47.               {
    
48.                   if(hz16[k].Msk[i*2]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    ' c$ `  O7 b& n
49.                 else {
    
50.                   if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
51.                 }
    
52.               }
    , |' @  e; M* x0 [
53.             for(j=0;j<8;j++)
    
54.               {
    9 g- m7 Y5 [; f( B) p. R0 y6 R. ?
55.                   if(hz16[k].Msk[i*2+1]&(0x80>>j))  Gui_DrawPoint(x+j+8,y+i,fc);
    + L! Q% W, H- q& L6 b
56.                 else
    : ?4 [' G" E& t
57.                 {
    1 Y# m- Q0 T1 o
58.                   if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);
    3 i- q& m5 d7 Z5 W$ p* ^5 N, I* C
59.                 }
    6 }8 W- ?3 Q4 P3 d0 U) z
60.               }
    
61.             }
    4 t( j2 m8 x) [0 I
62.         }
    
63.         }
    
64.       s+=2;x+=16;
    + y5 n. e2 J  K: }0 T& l
65.     }
    
66.    
    
67.   }
      c! X7 a- e5 [& u. B5 S, a
68. }
    # g& X  _6 e: m0 X, ?
69. </font>

复制代码

2.4 电机、舵机与编码器

% F8 }* A9 T( f
3 r/ f0 O4 p) K  c4 |' ~定时中断：TIM2，用于修改电机和舵机的PWM占空比
8 L- S) ]! @  ^初始化函数：

% s+ M. i' a! }& a; P7 t; [

1. <font face="微软雅黑" size="3">
   : t1 x- ~' \' R: q* i& ?
2. //通用定时器2中断初始化
   
3. //arr：自动重装值。
   % X, e, c( R! M2 }! j" ?
4. //psc：时钟预分频数
   ; ]% U% ]4 H0 H# @& D
5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
   5 n$ ]  _  h6 p, ^% G7 B% f! e
6. //Ft=定时器工作频率,单位:Mhz
   
7. //这里使用的是定时器2!
   7 }# z& d( Q' @; N# Y5 _2 U
8. void TIM2_Int_Init(u16 arr,u16 psc)
   
9. {
   
10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    
11.   NVIC_InitTypeDef NVIC_InitStructure;
    
12.   
    
13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
    1 G+ J8 K. ?* k
14.   
    ) a* I* z( I; C  ?6 y
15.   TIM_TimeBaseInitStructure.TIM_Period = arr;   //自动重装载值
    + l7 v% [3 ~6 N+ E/ y' w
16.   TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    & d. a( k) z& Q- j3 j: G
17.   TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
18.   TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    1 J2 S& r1 k1 ^) P; x0 @
19.   
    
20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
    3 G; D: N: F$ v) q
21.   
    - u7 _# \2 S+ c, f5 U- @
22.   TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断
    
23.   TIM_Cmd(TIM2,ENABLE); //使能定时器2
    
24.   
    
25.   NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器2中断
    , I& e$ r- Y% L+ s$ f8 M  i: W
26.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级1
    
27.   NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
    
28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    8 l$ G2 J' H+ ]
29.   NVIC_Init(&NVIC_InitStructure);
    + j7 {- e9 }" z: k; S7 B
30. 
    
31. }</font>

复制代码

TIM2中断处理函数：
% t. w! T9 n" Y! h' X' K) n0 R

1. <font face="微软雅黑" size="3">
   * w8 [4 Y& H' a; b) @" J. M1 I4 d6 N
2. void TIM2_IRQHandler(void)
   6 k4 K$ O" B; U! o6 k0 R
3. {  
   
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   3 Q" r7 n2 Z6 W0 U( i
5.   {
   
6.     if(motor_flag==1)//反转
   + {! F# V# F! k$ W
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   
9.       TIM_SetCompare2(TIM8,0);
   $ [) S4 P. C$ U) M$ x
10.     }
    
11.     if(motor_flag==0)//正转
    
12.     {
    % g( h* r* ?4 z9 ^- ]: k& @
13.       TIM_SetCompare1(TIM8,0);
    + F% d6 m# `! b' p9 S/ b  j; k9 Q
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    0 h; L1 T8 ?: d2 o
15.     }
    ) O5 S' H6 @9 U  t
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，引脚：PA6
    
17.   }
    
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    8 s) V& \* s; p. n7 U1 ]
19. }
    
20. </font>

复制代码

PWM输出：TIM3（舵机），TIM8（电机）
, ?. w& {. r) u8 A+ p2 R8 Y初始化函数（以TIM8为例）：
$ o  m3 J/ o, L; g; N& `/ B

1. <font face="微软雅黑" size="3">
   
2. void TIM8_PWM_Init(u32 arr,u32 psc)
   
3. {               
   
4.   //此部分需手动修改IO口设置
   
5.   
   
6.   GPIO_InitTypeDef GPIO_InitStructure;
   8 J1 w8 W2 u0 d; Q1 G6 |1 S
7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   
8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
   1 l8 u3 l4 Z* i5 W* r* o+ A' I
9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
   
10.   
    7 P; q+ Y- T, L( N7 t, t" \$ w3 n6 M
11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
    
12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
    
13.   
    / _6 q! F) }: m* t" @+ |
14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
    
15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    
16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
    
17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    % X0 [- F; Q/ E0 t
18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
    : j* G4 O% ~4 G9 X. I! j
19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7
    8 S& G6 `/ W7 {3 X# A
20. 
    
21.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    
22.   GPIO_Init(GPIOB,&GPIO_InitStructure);
    2 I! P% ?. w; p9 }( l, T
23.   
    , B3 s: n/ _* ?! g' x
24.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
    $ K7 @" q& L- }! e! M# O1 O! g
25.   GPIO_Init(GPIOC,&GPIO_InitStructure);
    0 e8 a; J$ r6 G* M
26.   
    , ^) J. H# e6 `# Q
27.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
    ' S8 F& k; i2 q7 Z' L1 ]* P
28.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1
    , A( t3 z0 K# h  n
29.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
    ( h  @' X7 Y1 n+ T, `: M7 k* ^( q
30.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13复用为定时器1
    
31.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14复用为定时器1
    0 ^2 Q$ ?7 c* c6 i5 o% n
32.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15复用为定时器1
    
33. 
    
34.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
    
35.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
36.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
    
37.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
38.   
    2 v  d8 j+ z; Q
39.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
    
40.   
    . g1 l+ ^" Q. [- N
41.   //初始化PWM模式   
    
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;
    0 m& f8 O- [" y/ C3 a
46.   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    
47.   TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
    4 F. t! q  |  S. E4 B8 V
48.   TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
    9 \: P! w- A- n( T' ]1 N9 y. @' l
49.   TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
    
50. 
    & z) s  o3 E1 q5 d8 P
51.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    2 q0 v& E% X( @
52.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    . s9 l% q+ p% o& p$ [
53.   TIM_OC3Init(TIM8, &TIM_OCInitStructure);
    
54. 
    
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;
    
61.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
    
62.   TIM_BDTRInitStructure.TIM_DeadTime = 0;
    
63.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
    
64.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;
    & J9 z. c& t% E9 f+ I
65.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
    2 e" P3 `* M  g# V- k7 {- W
66.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);
    5 g. i( m1 z( {9 S2 |+ Y4 ^
67. 
    / H" t3 }6 l3 |) A5 u, e, I& u
68.   TIM_Cmd(TIM8,ENABLE);
    
69.   TIM_CCPreloadControl(TIM8,ENABLE);
    
70.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
    
71.                      
    
72. }</font>

复制代码

编码器初始化函数：
) r, F" l/ r' n6 V, o& `

1. <font face="微软雅黑" size="3">
   ) M, p( U5 b" O3 G6 S
2. void Encoder_Init_TIM4(void)
   * _7 Q& R  s9 S! M
3. {
   
4.     GPIO_InitTypeDef         GPIO_InitStructure;
   
5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   1 k8 Z0 b3 x( Q- {! j1 }' c
6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
   " m* T# j0 s  W- o! f) c
7.     NVIC_InitTypeDef NVIC_InitStructure;
   ) R3 M' g; M. S5 _
8. 
   + \, B& [6 a/ j2 {' }- I
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引脚复用
    - A) Z! |+ X4 q) {! i
13.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用
    & f( b3 W" N9 X8 X
14. 
    # I: n/ L- o: [8 Y& I& H7 J
15.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
    : L7 x0 m6 V/ x5 m3 a/ e) V
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 ;
    ( C& _) ?$ [4 h
20.     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
    , I3 U6 i* C; ]- a  g6 e& ~
21.     GPIO_Init(GPIOB,&GPIO_InitStructure);
    
22.    
    ' W; i! E  J* P
23.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    
24.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
    ) r, _! _2 ?, U: e3 B7 M
25.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    : @/ C. u; K# _( `9 `. m
26.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断，防止计数溢出而没有相应函数，造成卡死
    9 r: q6 e' {/ a1 X  x3 n
27.     NVIC_Init(&NVIC_InitStructure);
    
28. 
    
29. 
    
30. 
    
31.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
    7 h9 b- A8 ~% Z3 x# E* S
32.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
    $ Z( }0 }3 X1 D3 k% _: @0 y
33.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
    
34.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    , X% F2 y; U7 W+ H& T, a
35.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    2 |* p* X% Q- \6 |# j
36. 
    " C8 s; \- ]  Z6 o$ ~( a
37.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
    
38.     TIM_ICStructInit(&TIM_ICInitStructure);
    0 ]/ Z5 x: A8 x' n" f. r: R  V
39.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
    4 t. R+ i$ N* K6 H8 A2 u
40.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
    
41.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
    
42.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
    9 P7 _8 [& C' E4 ]6 |
43.     TIM4->CNT = 0;
    . V, g1 p% O' o3 J  y
44.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
    3 n% c% j7 I; z+ ?7 z
45. }
    0 l6 I4 C5 w" j+ v' I; x
46. </font>

复制代码

编码器返回速度值：

1. <font face="微软雅黑" size="3">/**************************************************************************
   6 ]- c- |; r& q
2. 函数功能：单位时间读取编码器计数
   ! A# L  i! P0 |" K  D( m% M& X. V7 i
3. 入口参数：定时器
   
4. 返回  值：速度值
   % w, U2 E& t' k' _$ |; A/ ^6 q# |
5. **************************************************************************/
   
6. float Read_Encoder_Speed(uint8_t TIMX)
   7 n# h/ ?/ p$ {3 k9 H
7. {
   
8.     int32_t Encoder_TIM;
   6 e9 ?. ~, d; Z6 F
9.     float res = 0;
   ; V% N8 q$ j/ A' V; K
10.     switch (TIMX)
    
11.     {
    
12.     case 5:
    
13.         Encoder_TIM = TIM_GetCounter(TIM5);
    
14.         TIM5->CNT = ENCODER_BASE_COUNT;
    
15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    / w; K* ?& N" G( A' p( f% b
16.         break;
    2 [+ I$ s! Y( c; C' i/ B7 n: P. e. C
17.     case 4:
    
18.         Encoder_TIM = TIM_GetCounter(TIM4);
    
19.         TIM4->CNT = ENCODER_BASE_COUNT;
    $ Y) ^8 P1 I5 \1 d* X, P
20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    
21.         break;
    
22.     default:
    " |. A- V* s  d+ i" D
23.         Encoder_TIM = 0;
    + [1 H' R: @# f) T2 ]
24.         res = 0;
    
25.     }
    " B% @. E" [% x1 M8 t7 T9 g7 j  ~, u' u
26.     if(res>2048.0f)
      d, w( m! e+ w% p+ ]
27.         res-=4096.0f;
    4 D+ w7 Q$ W- U" l6 \
28.     return res*360.0f/4096.0f;
    " H! A% B5 r% r" h- R
29. }
    
30. </font>

复制代码

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

1. <font face="微软雅黑" size="3">
   
2. void TIM7_IRQHandler(void)//频率20HZ，用于编码器计数
   
3. {  
   
4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   
6.     speed=Read_Encoder_Speed(4);
   
7.   }
   
8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
   / w1 Y( K* p( y2 U( @+ V! o
9. }</font>

复制代码

2.5 PID控制
  i9 e2 E$ j+ @& L& {! d. i0 |* |
$ c5 v: {* t. X+ R6 _  q$ }2 k
, n& z, e6 B  n$ `) a$ _. JPID库函数：

6 x$ P% N; T" a% Z4 S- ~' E4 _" S. w

1. <font face="微软雅黑" size="3">
   
2. #define N 2        //需要对多少变量进行pid调节
   # T9 y/ [0 v1 Z& s2 w/ `
3. 
   
4. const float KP[N]={1.3,1.0};//这里只用了比例调节
   + T3 k7 ?1 `+ I
5. const float KI[N]={0,0};
   
6. const float KD[N]={0,0};
   
7. 
   / C% H1 m* p3 _4 v: C+ O; Q
8. 
   # h) O+ c( G( C, V6 A% l
9. struct _pid{
   
10.   float SetVol;        //定义设定值
    : n! e# ^2 S" \
11.   float ActVol;        //定义实际值
    
12.   float Err;          //定义误差
    
13.   float  Err_Next;      //定义上一个误差
    7 G6 L$ w$ U/ T4 S/ ]  U
14.   float  Err_Last;      //定义上上一个误差
    
15.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
    + \8 i) h2 M% e; G3 d% v" j
16.   float integral;      //定义积分值
    % {. @. Q, m* D+ Y9 p4 ?
17.   float actuator;      //定义控制器执行变量
    
18. }pid[N];
    9 s5 N0 X. h  G% P  K3 F% Q5 ?9 a
19. 
    
20. void PID_Init(void)
    
21. {
    ) H* O/ E! W4 f
22.   for(int i=0;i<N;i++)
    : H) q- }) @/ i
23.   {
    
24.     pid[i].SetVol=0.0;
    
25.     pid[i].ActVol=0.0;
    ; C9 w4 B& S, t% ?8 N8 U
26.     pid[i].Err=0.0;
    1 G0 Z5 ^( U7 j1 w8 m5 _
27.     pid[i].Err_Next=0.0;
    2 V& E/ q" c2 Y& M) S1 r7 z9 Y  \
28.     pid[i].Err_Last=0.0;
    
29.     pid[i].integral=0.0;
    
30.     pid[i].actuator=0.0;
    
31.     pid[i].Kp=KP[i];
    
32.     pid[i].Ki=KI[i];
    3 }1 ]& \7 A6 ~+ U; l2 N, K
33.     pid[i].Kd=KD[i];
    / c% {1 w; Z, \' x" @  K5 e( F
34.   }
    7 y+ w' X6 r5 t3 X1 F
35. }
    
36. 
    
37. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
    
38. {
    ! C+ Z$ ?+ N' D: g. a* q/ A4 Z
39.   pid[i].SetVol=set_val;
    
40.   pid[i].ActVol=get_val;
    
41.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
    
42.   float IncVol;    //定义增量
    
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);
      H5 Z& `! |& U8 B* F* Z1 g# D* o  h8 P
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);
    + m( e' _" J3 p
46. //  pid[i].actuator=adc_val+IncVol;
    ' O5 W, ~* k0 P$ S: U1 N3 H
47.   pid[i].ActVol=pid[i].actuator;
    " D0 {$ }, x- H
48.   pid[i].Err_Last=pid[i].Err_Next;
    
49.   pid[i].Err_Next=pid[i].Err;
    " c4 _+ J* f2 T1 q
50.   
    
51.   return pid[i].actuator;
    
52. }</font>

复制代码

主函数中的PID调节：

& e( N+ Z, b4 G

1. <font face="微软雅黑" size="3">
   6 W# k) K2 M( j$ [+ }
2.     z_get=data[2];
   8 o/ _9 l" M( J4 J
3.     x_get=data[0];
   
4.     if(z_get-z_set>0.5||z_get-z_set<-0.5)//电机PID
   
5.     {
   
6.       LED1=0;  //调节时灯亮
   
7.       PID_val_motor=PID_realize(z_set,z_get,0);
   - M; A. [( H0 i7 U! H' Z" U
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)
    2 o, f$ Q& C5 ]; P0 D
12.         motor_flag=1;
    2 i" Z  l! ~0 K% ]) Q: ?
13.       PID_val_motor=abs_float(PID_val_motor);
    
14.       if(PID_val_motor>2)PID_val_motor=0;//标志太远，让车停止
    . g$ q4 Z& |7 q/ E5 G& H% |: i" F
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;
    0 @9 G- D! t3 E9 r
17.     }
    - ?) B5 x+ @& p/ S; U5 G
18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
    
19.     {
    . f: a' c7 j! G. D9 L9 D6 t8 S
20.       LED1=0;  
    
21.       PID_val_servo=PID_realize(x_set,x_get,1);
    
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;
    
25.     }
    6 H: v4 c$ W. l+ N
26.     LED1=1;</font>

复制代码

定时器TIM2中断里改变占空比：
+ X0 z, z) }6 a2 N) X% k

1. <font face="微软雅黑" size="3">
   0 D+ }' ^5 k; V% C5 n9 t
2. void TIM2_IRQHandler(void)
   # a8 l$ {1 Y- \6 M8 p' \
3. {  
   
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   , ^+ K  J$ h+ q1 e7 ~
5.   {
   
6.     if(motor_flag==1)//反转
   1 V- D1 V1 b: L& ]9 P. C! b9 M& J. \
7.     {
   & m0 S* h4 d5 @
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   , ?  ~5 Q) \: f
9.       TIM_SetCompare2(TIM8,0);
   
10.     }
    
11.     if(motor_flag==0)//正转
    % X' ?6 Q" f3 Z; W0 G8 }/ O2 ^
12.     {
    6 y5 M# Y0 o0 y) E
13.       TIM_SetCompare1(TIM8,0);
    " Q+ d7 f# j% `
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    
15.     }
    : _1 `' C" p; y" U- [, N
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，根据舵机手册得到占空比与转角的关系，引脚：PA6
    
17.   }
    7 v& m/ k, r% F5 D
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    8 w& j8 U! M. I9 B# R
20. </font>

复制代码

$ f/ R7 m0 s! y2 f% T- y

优秀