概述
小车外形：

$ b' U$ z; _3 W$ |  G, j

功能简介
& R# d6 o; w) ~; p利用摄像头识别前车尾部的AprilTag，得到前车位置，传回stm32主控板处理，使两车在行驶时保持恒定距离，实现自动跟车。


4 }: k& W, h1 Y0 o" y, p2 e

" r& M1 h& j1 g8 m4 TopenMV4摄像头
1.1 Apriltag识别与串口传输
: V* ^9 C8 N* D! A6 ^9 p
5 k: h( H- J: @2 E- s4 Q* ], h' f
AprilTag是一个视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志（与二维码相似，但是降低了复杂度以满足实时性要求），可以快速地检测标志，并计算相对位置。
1 ?( ]5 j: {/ Q! e# p

0 `2 _8 [" |+ n8 |* HApriltag示例：

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

1. <font face="微软雅黑" size="3">
   , l, ~0 @4 i3 w
2. import sensor, image, time, math,pyb
   $ T0 ?/ X! y( W8 R
3. from pyb import UART
   
4. 
   7 y# h# K' {! o  ]- \0 Z: Z% @+ T
5. sensor.reset()
   
6. sensor.set_pixformat(sensor.RGB565)
   # b/ E! q5 d! [! ~( z6 f
7. sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
   
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...
    " \6 t. z* m; ]: C7 x
11. clock = time.clock()
    ) y: G+ z8 p3 n, @+ @
12. uart = UART(3, 115200)#串口波特率
    4 H* d! {. P9 A/ y9 F
13. 
    
14. f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
    ! ~; ?+ }9 j) U9 O
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)
    
17. c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
    
18. 
    
19. def degrees(radians):
    
20.     return (180 * radians) / math.pi
    5 Q/ ?  }8 x/ }4 z% S; o0 a$ h
21. 
    
22. while(True):
    
23.     clock.tick()
    
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
    " m/ \. G" ^: ]$ X9 @. K
26.         img.draw_rectangle(tag.rect(), color = (255, 0, 0))
    7 i1 |# x/ X$ }* b8 T
27.         img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
    9 R, X; w; r: B% H. X$ q, G( w% @2 W
28.         print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
    " I. E* d5 V+ ^; ^) H
29.             #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
    5 e5 U, L; g7 X" L$ R
30.         # Translation units are unknown. Rotation units are in degrees.
    
31.        # print("Tx %f, Ty %f, Tz %f" % print_args)
    ( |, p- ]# Z# Q* D. X; V
32.         uart.write("A%.2f,B%.2f,C%.2f," % print_args+'\r\n')#设置特定格式，以便于stm32分割取得数据
    7 B. B4 T, G$ t# m+ y- U2 I
33.         #pyb.delay(500)
    
34.    # print(clock.fps())</font>

复制代码

STM32主控板（型号为F407）
* N4 [; y$ M9 T7 Q

( z- Y+ |  }! i) s0 R7 l2.1 时钟与中断配置
3 l1 O; _* ^7 [8 S
5 ]% J# ~) l/ c
+ B. I+ b- u4 P' T附上stm32时钟示意图：

定时器示意图：
9 e% D! [' L) l! p' b, t$ n8 k  I! r
定时器分配：
) B- w* o1 P6 W/ _
所有时钟初始化的函数：（每个函数的详细内容在后面）

1. <font face="微软雅黑" size="3">
   
2. TIM8_PWM_Init(400-1,20-1);  //用于控制电机，168M/20=8.4Mhz的计数频率,重装载值400，所以PWM频率为 8.4M/400=21Khz.
   3 e: I& D  B* c
3.   TIM3_PWM_Init(200-1,8400-1);//用于控制舵机，50HZ
   
4.   TIM2_Int_Init(400-1,20-1);//定时中断，21KHZ
   7 Z9 w; ^9 W8 ]9 ^9 J
5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数，20HZ，50ms中断一次
   
6.   uart_init(115200);  //初始化串口1波特率为115200
   
7.   Encoder_Init_TIM4();//编码器接口初始化</font>

复制代码

2.2 串口收发与数据处理
! O! I) x: q. P% h+ c5 J

串口中断：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;
   ; Y8 t" _# K8 w( q' n
6.   NVIC_InitTypeDef NVIC_InitStructure;
   7 c  r& h  r: u! X6 K& B
7.   
   0 v- Z& I$ G, P& s/ I+ r* w
8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
   
9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
   4 m7 ?  @4 i* i3 A- {
10. 
    
11.   //串口1对应引脚复用映射
    % q! R' ~% S  o+ V/ ~3 D1 ]! {
12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
    7 j- a: ]; I: i$ m4 H. B
13.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
    
14.   
    5 c. _) ?9 ~( E
15.   //USART1端口配置
      B! e8 H" s9 B* q, u
16.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
    
17.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    
18.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度50MHz
    ; B8 x. }+ }5 \7 H" k$ ?
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 初始化设置
    3 Z  p0 p# y' }9 a9 a! {# g. X
24.   USART_InitStructure.USART_BaudRate = bound;//波特率设置
    
25.   USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
    $ s/ D$ S- \( L: `6 \+ Q
26.   USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
    6 e% u/ z8 _6 \4 L
27.   USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
    9 Z0 l( d8 e$ {- x
28.   USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
    
29.   USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;  //收发模式
    8 C  k' o: E4 F5 D( b) K
30.   USART_Init(USART1, &USART_InitStructure); //初始化串口1
    ' q8 C" B7 P/ \. j
31.   
    2 {5 ^/ Z) h  M' ^) F1 Z
32.   USART_Cmd(USART1, ENABLE);  //使能串口1
    & w# L* r  \3 L+ k6 ~
33.   
    
34.   USART_ClearFlag(USART1, USART_FLAG_TC);
    
35.   
    
36. #if EN_USART1_RX  
    
37.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
    " U, C6 [4 v, y- M: `
38. 
    2 _) v3 b* _: M+ w2 X
39.   //Usart1 NVIC 配置
    
40.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
    2 p& f1 L9 x- p& y- h7 Y2 I
41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
    
42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
    2 v& `+ x. k: P) {* |
43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
    ! y  W# E- ~; {$ ^2 {* Y7 I* A# `
44.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、
    
45. 
    
46. #endif
    3 @+ r5 I8 c1 L, n' Q. I: k
47.   
    
48. }</font>

复制代码

串口中断处理函数：

1. <font face="微软雅黑" size="3">
   : W& ^/ a. r( g9 b  {! A
2. void USART1_IRQHandler(void)                  //串口1中断服务程序
   ! {' n, B) |8 ~  F! n. u
3. {
   
4.   u8 Res;
   2 e4 Z  t7 P- S% Z# w. j
5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
   
6.   OSIntEnter();   
   2 x) w9 H: E' X, c9 B0 w' w8 B% V
7. #endif
   
8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
   / }# F# U, _$ w; `
9.   {
   
10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
    
11.    
    * ~% j9 u' X- Q- h$ M* h. n9 t
12.     if((USART_RX_STA&0x8000)==0)//接收未完成
    5 B$ F' r% S7 j' X' s3 Z
13.     {
    
14.       if(USART_RX_STA&0x4000)//接收到了0x0d
    . E& w/ i! f* z$ Y& ?# z& `; h$ {
15.       {
    
16.         if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
    / S" Y. q+ U+ V
17.         else USART_RX_STA|=0x8000;  //接收完成了
    
18.       }
    
19.       else //还没收到0X0D
    / r- b% n; f0 j1 g7 a
20.       {  
    
21.         if(Res==0x0d)USART_RX_STA|=0x4000;
    
22.         else
    . n# ]% ^6 U' u+ c
23.         {
    
24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
    
25.           USART_RX_STA++;
    0 d- |8 l2 D* D( q2 w& A, ^- u
26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
    7 W; G+ Y* N  R- R; L
27.         }     
    
28.       }
    
29.     }        
    2 W5 B( O$ O3 V5 G' K6 I
30.   }
    % U9 ^% J" _" J/ X) n! d
31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
    
32.   OSIntExit();                        
    3 \; Q3 n% S# [( u% z) Y+ b, t
33. #endif
    ( V/ A3 X6 q  G! v
34. }
    4 @8 k# H% i; e1 f+ Q5 i
35. #endif  </font>

复制代码

字符串接收与处理（从openMV接收到的数据）：
- A- }  E' i$ ?4 _) @" r+ n  ~+ y

1. <font face="微软雅黑" size="3">/*涉及到的全局变量
   - {( i8 Y# H1 O1 W' r6 O& o  {
2. float data[3];//x,y,z方向的距离，浮点数形式
   + _. C- x# m3 ~7 w5 Y7 b8 y
3. unsigned char data_string[3][7];//x,y,z方向的距离，字符串形式
   
4. */
   / v0 c1 V& ]7 X0 Z* f! w
5. if(USART_RX_STA&0x8000)
   
6.     {  
   2 b7 N# e- U. N# b: A, p2 Z
7.       //清空字符串
   
8.       for(i=0;i<2;i++)
   ' S* t8 T2 O) n. d9 ^8 c0 `! [
9.       {
   
10.         for(j=0;j<6;j++)
    ; b* Q' t. \1 U3 X; s2 H. f
11.         {
    
12.           data_string[i][j]=' ';
    ) u$ s( r+ w& o2 H7 d  M! R& W
13.         }
    
14.       }
    + A# O1 r) d5 F" j6 b
15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
    
16.       for(t=0,j=0;t<len;t++)
    3 B8 k5 V% A! S8 G
17.       {
    4 M9 G# N) o6 J7 h" w
18.         j=0;
    8 O0 J7 u0 O" Z
19.         if(USART_RX_BUF[t]=='A')
    * B# G) d% _: F* i! e) {$ b) e
20.         {
    
21.           t++;//去掉首字母
    
22.           while(USART_RX_BUF[t]!=',')
    ; n) r! s/ q& _" N* J2 T) e
23.           {
    / t6 \9 s3 {' k- `% ~" j
24.             data_string[0][j]=USART_RX_BUF[t];
    9 Y8 K7 h' _, V9 I
25.             t++;
    
26.             j++;
    
27.           }
    ! e( ]8 V( v: G, V; D  m* h
28.         }
    ' S3 [0 y; L- G$ O9 i- G, B
29.         if(USART_RX_BUF[t]=='B')
    0 P8 E& i7 F- O
30.         {
    
31.           t++;//去掉首字母
    - |; T, k1 b- E( b7 N! V& C
32.           while(USART_RX_BUF[t]!=',')
    ' y0 o  f6 H, y  l9 F( c" a$ ^
33.           {
    0 a9 h) K1 P) s
34.             data_string[1][j]=USART_RX_BUF[t];
    
35.             t++;
    
36.             j++;
    
37.           }
    
38.         }
    6 h& s! V8 h& x) W* n
39.         if(USART_RX_BUF[t]=='C')
    
40.         {
    
41.           t++;//去掉首字母
    
42.           while(USART_RX_BUF[t]!=',')
    # |& d5 Z. H2 n( i; \- E, F
43.           {
    ( M& X5 A  ~) k
44.             data_string[2][j]=USART_RX_BUF[t];
    " o7 a8 Q! [3 o$ r+ `  v' J  i7 S& g
45.             t++;
    + m6 Q' d  O/ u& y1 w3 p0 _  O# Q
46.             j++;
    
47.           }
    
48.         }
    ' c5 r% }6 `2 Y3 t' h
49.       }
    
50.       //把字符串转化为浮点数
    ( s, T8 Y0 R  }! E7 |! U
51.       data[0]=myatof(data_string[0])/100.0;//x
    $ S7 E# z$ d! [) L, K: }# [( A. b
52.       data[1]=myatof(data_string[1])/100.0;//y
    0 f7 j! q* a2 U( w
53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z，输入是负数，转换为正
    # [; x* j- i9 p9 }- u& o) d
54.         //USART2发送数据
    % Y% K5 e8 N8 y% P1 `  |
55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
    5 r( w: J0 d  K! r0 |  L1 F
56.         
    ; r" K- f* v; [, O9 q
57.       //LCD更新显示
    . @' J5 B  q7 o0 p" F% G
58.       //显示xyz
    
59. //        CLEAR(10);
    2 _5 r1 I2 u1 |; {
60. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);
    2 Z" F5 x4 j) y6 j, ]0 U
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]);
    & s9 V$ T# F5 x8 t/ n# S7 P+ ^, A' J# e
64. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
    
65. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);
    2 F# N8 H, K  \
66.         
    
67.         USART_RX_STA=0;//清除标志位
    
68.     }
    
69. }
    
70. </font>

复制代码

字符串转化为两位小数浮点数（用于后续PID控制）:
- C9 p+ g1 c* I- t  c

1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数，返回的是乘100后的int值，因float返回有错误
   / y. ~: K  v0 _9 W' U. Z. [
2. {
   
3.   int flag = 1;//表示正数
   
4.   int res =0;
   
5.   u8 i=1; //小数点后两位
   ' W; j/ V; y. t  t
6.   while(*str != '\0')
   3 p; x2 v/ k( n( p/ p
7.     {
   
8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
   * N) ?  c% F0 _8 y
9.       {
   - n. Q$ V' ?! ^# t% A5 u8 x! g
10.         str++;
    
11.         continue;
    ( c* f! q! @- M
12.       }
    
13.       if(*(str-1) == '-')
    
14.       {
    , O7 p! T. g' V
15.         flag=-1;//表示是一个负数
    
16.       }
    
17. 
    
18.     while(*str >= '0' && *str <= '9')
    8 N% ~9 M% U$ R) G+ r: r
19.     {
    
20.       res = res *10 + (*str - '0');
      H* E+ w* `) d
21.       str++;
    
22.     }
    4 w6 w3 K: h* ~* L$ k
23.     if(*str == '.')
    5 o7 S4 i# K4 B* H
24.     {
    
25.       str++;
    
26.       res = res *10 + (*str - '0');
    ' q( R/ f8 I/ m- ^$ \4 r
27.       str++;
    
28.       res = res *10 + (*str - '0');//保留两位，故加两次
    
29.       return res*flag;
    + T' G, k1 {! \9 h1 f3 z
30.     }
    
31.     }
    6 y: }3 O  }8 j) ^* M  _
32. }
    ; m! X3 o6 p. K7 k' O
33. 
    ; s& }* t0 Y6 h$ \& Y3 E$ v% A
34. 
    
35. 2.3 LCD显示模块
    
36. 
    
37. LCD模块用于调试时观察数据，调试完成可以删去，因为显示屏很耗时，使处理速度变慢
    
38. 驱动函数总览：
    3 S; L; T8 h' f+ y
39. 
    
40. 
    + W; d& F! e1 {) ~9 j  f
41. 
    
42. void LCD_GPIO_Init(void);
    . r2 x5 _/ m. Z! r! ~
43. void Lcd_WriteIndex(u8 Index);
    8 }3 y2 A6 `% x$ z- |; x, Q
44. void Lcd_WriteData(u8 Data);
    
45. void Lcd_WriteReg(u8 Index,u8 Data);
    * [6 \6 I5 E% C4 @5 o* B# w
46. u16 Lcd_ReadReg(u8 LCD_Reg);
    + o# i8 |$ Q- R8 E5 Q  R2 K# j
47. void Lcd_Reset(void);
    
48. void Lcd_Init(void);
    
49. void Lcd_Clear(u16 Color);
      G/ }$ _8 l' o- J/ ?+ m" x: S0 ?
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);
    8 S1 |, }- ~) f3 V( Z% M: z( f
54. void LCD_WriteData_16Bit(u16 Data);</font>

复制代码

  E9 }- c1 A# e" P
; C; e4 b8 c, v4 {- _TFT屏幕初始化：
; z7 T/ b( B* D7 e% b
9 {5 I- u% z! M. K" u$ H

1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
   
2. {
   
3.   Lcd_Clear(WHITE);
   
4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
   
5.   delay_ms(1000);
   # v2 T# V* w" u" ~9 b
6.   Lcd_Clear(WHITE);
   9 c! ?2 b- C7 U! N& T4 m3 Y4 d
7.   Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
   
8.   Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:");
   # s1 O9 A5 O, G7 E& l3 h3 \
9.   Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:");
   
10.   Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");
    ( M5 J+ @- [* l+ k- E$ o2 J
11. }</font>

复制代码

字符串显示函数;
+ n! d) {) ^6 Y8 N" H! s

1. <font face="微软雅黑" size="3">
   
2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
   & O$ p( E/ G3 s" H
3. {
   ) {- m! _- b) s
4.   unsigned char i,j;
   1 K2 s6 G( g# D1 a
5.   unsigned short k,x0;
   3 Q4 u' w  i6 O
6.   x0=x;
   
7. 
   
8.   while(*s)
   
9.   {  
   + d2 A! M# z0 ?
10.     if((*s) < 128)
    6 h1 N; Z; B- x* r" H9 {
11.     {
    / @% D. O# k1 ]4 [, t3 r* G6 T
12.       k=*s;
    
13.       if (k==13)
    9 M, e. {1 R4 a
14.       {
    $ R- Y  M# h6 E/ [9 O$ h
15.         x=x0;
    
16.         y+=16;
    
17.       }
    / a2 n5 N) V6 d5 c2 D( O4 Z  W, d
18.       else
    
19.       {
    
20.         if (k>32) k-=32; else k=0;
    
21.   
    
22.           for(i=0;i<16;i++)
    
23.         for(j=0;j<8;j++)
    4 P4 J, T& D+ X
24.           {
    % N0 D0 b" N& q4 K
25.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
26.             else
    
27.             {
      a! M4 o8 y7 Z: L0 s4 {/ ]
28.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    9 c! I9 ^) S& R1 m7 m: ]
29.             }
    
30.           }
    3 \& [# ~% {. c! o6 X5 M
31.         x+=8;
    
32.       }
    8 ]  Z/ {& u; w
33.       s++;
    
34.     }
    
35.       
    
36.     else
    
37.     {
    9 u  [2 c4 e6 |1 [! X
38.    
    " x) v% h0 ^! x. z# ?9 v
39. 
    & Z; h+ G' j* l+ O0 n4 C& e
40.       for (k=0;k<hz16_num;k++)
    / i5 q# v* `# N- h- a
41.       {
    9 i6 I5 m  [& Y% D- S
42.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
    . f2 s7 `) }) v  H. i$ o
43.         {
    
44.             for(i=0;i<16;i++)
    
45.             {
    
46.             for(j=0;j<8;j++)
    * C3 F& j6 \0 Y7 L* n5 f2 B2 r+ m
47.               {
    : O9 q; {. T) x# R$ n
48.                   if(hz16[k].Msk[i*2]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
49.                 else {
    5 Y4 M/ k7 o+ G
50.                   if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
51.                 }
    
52.               }
    , E4 B3 _% [+ m
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);
    
56.                 else
    
57.                 {
    / Y3 c; `) _9 ?7 q0 t- `0 z) j
58.                   if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);
    . ^" N& W  v5 B  C& _& S
59.                 }
    9 Y* E2 T) y: x' P/ m& W; Q2 S
60.               }
    
61.             }
    , Q/ B& L) w/ E4 A
62.         }
    
63.         }
    
64.       s+=2;x+=16;
    
65.     }
    
66.    
    
67.   }
    
68. }
    2 x+ m3 T" {% M) V9 {0 {
69. </font>

复制代码

2.4 电机、舵机与编码器


定时中断：TIM2，用于修改电机和舵机的PWM占空比
- h" F' s2 ~% ~3 f$ y初始化函数：
; W2 D& f7 |/ U4 G+ v
7 d- L4 `, }; [: Q( r5 j

1. <font face="微软雅黑" size="3">
   
2. //通用定时器2中断初始化
   : k: t  R. C6 d0 ?: D# C
3. //arr：自动重装值。
   
4. //psc：时钟预分频数
     D. I8 A6 a5 G+ w9 U2 \
5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
   
6. //Ft=定时器工作频率,单位:Mhz
   
7. //这里使用的是定时器2!
   
8. void TIM2_Int_Init(u16 arr,u16 psc)
   & x! `- H5 z; e
9. {
   2 Y# Q" D) A/ c3 g7 j
10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    ; _( E# Q( L7 p# v: f
11.   NVIC_InitTypeDef NVIC_InitStructure;
    
12.   
    3 B9 F4 S1 A6 `2 r
13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
    ! Y& K. U" z1 p$ O
14.   
    , @' ?  a; ^& P  ~
15.   TIM_TimeBaseInitStructure.TIM_Period = arr;   //自动重装载值
    
16.   TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    4 ?2 F9 o2 u8 z5 b- O' n# L* \
17.   TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    * G! M+ a5 C6 u; z% l" P$ ^
18.   TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
19.   
    ' m6 k# F  K% ~# h5 b7 |3 n0 `
20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
    7 j+ h, c0 A3 {' R2 n
21.   
    
22.   TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断
    
23.   TIM_Cmd(TIM2,ENABLE); //使能定时器2
    , M* E6 w  n# ^- Z/ {
24.   
    
25.   NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器2中断
    
26.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级1
    
27.   NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
    
28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    
29.   NVIC_Init(&NVIC_InitStructure);
    . _. Y$ F2 T9 f/ m. M( Q* J
30. 
    
31. }</font>

复制代码

TIM2中断处理函数：
3 r. O- l2 d8 ^0 \3 y9 e; n

1. <font face="微软雅黑" size="3">
   % G  R% ^& S3 I" l) o: k
2. void TIM2_IRQHandler(void)
   
3. {  
   
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   2 K0 n3 n- ?/ {  e
5.   {
   , v2 C0 ]9 u2 A* |* A, p1 P! B
6.     if(motor_flag==1)//反转
   
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   ; ?0 x% u/ M, B' @+ t
9.       TIM_SetCompare2(TIM8,0);
   ( \8 I- B* B) G
10.     }
    ) T1 y, T* y) p6 ?3 {2 Y' t
11.     if(motor_flag==0)//正转
    - J. R4 M; d- _) L! U" b
12.     {
    1 _7 X! E4 U- S
13.       TIM_SetCompare1(TIM8,0);
    ) Y( H6 t4 i$ Q. f1 M' c
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    9 _9 {; B4 s, X- G+ H
15.     }
    
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，引脚：PA6
    
17.   }
    4 u5 X& [* O" E! T! W1 d
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    ' n, L* ?% U; Q/ |
20. </font>

复制代码

PWM输出：TIM3（舵机），TIM8（电机）
( W: c9 r5 k, q( a4 Y8 |初始化函数（以TIM8为例）：
+ P) {$ r, M9 c+ [) L+ R2 j* R

1. <font face="微软雅黑" size="3">
   
2. void TIM8_PWM_Init(u32 arr,u32 psc)
   # p5 p! |2 f# G
3. {               
   
4.   //此部分需手动修改IO口设置
   ; P: b+ c2 ^+ I$ A9 J3 h. t
5.   
   
6.   GPIO_InitTypeDef GPIO_InitStructure;
   
7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   8 J- V2 H. n" W# _' A
8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
   
9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
   
10.   
    " N1 j  [$ c' k! e2 G
11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
    
12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
    
13.   
    8 c% H7 R  q" G* @0 m/ r4 |6 ]1 U5 ^
14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
      A/ \7 T/ {. u# L  Q# L5 [
15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    
16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
    " O8 W7 l0 {+ m' Q
17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    5 g  k! V% s3 l: D( ?' m
18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
    5 {% N4 A$ f. x$ V- W5 C
19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7
    8 Q+ h& j5 o- s/ ]
20. 
    
21.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    
22.   GPIO_Init(GPIOB,&GPIO_InitStructure);
    & K  H- N2 l! ?) ^( y
23.   
    ) }6 A" N6 ?& p4 v! L
24.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
    
25.   GPIO_Init(GPIOC,&GPIO_InitStructure);
    3 F) ]* L- i" m. p* D
26.   
    , V+ Q6 W3 F' U% @2 L
27.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
    1 `( C0 p) M9 d& c$ x+ i5 @
28.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1
    
29.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
    , h/ l4 q; P5 ?* }% ~
30.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13复用为定时器1
    8 ^/ Z, }0 ~) u$ X$ f
31.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14复用为定时器1
    # m* M* L( L! S0 T; ]
32.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15复用为定时器1
    - `6 \' o: y+ i$ q, U" R
33. 
    
34.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
    
35.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
36.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
    4 u& X3 m5 g; q# A( Q  J; V
37.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    + k; {- t- K: V/ |
38.   
    7 ?' N. w  e8 s
39.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
    
40.   
    # s/ Y5 n2 R( |2 Q
41.   //初始化PWM模式   
    
42.   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    & A# `2 V- ^& L. y+ C9 C2 X
43.   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    + b3 e( ^2 V: v& T' ?
44.   TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    : Z, l1 |& t6 p3 x& E$ i: j
45.   TIM_OCInitStructure.TIM_Pulse = 0;
    
46.   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    
47.   TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
    
48.   TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
    . ^( _( Z% x; y/ [  H! |
49.   TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
    
50. 
      o5 n8 Q5 w( }/ \9 N7 b
51.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    
52.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    + N# }  g+ k9 G' V4 t
53.   TIM_OC3Init(TIM8, &TIM_OCInitStructure);
    
54. 
    
55.   TIM_OC1PreloadConfig(TIM8,TIM_OCPreload_Enable);
    4 T% J4 @& _& }; n( @
56.   TIM_OC2PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
57.   TIM_OC3PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
58. 
    - B8 {$ J1 \8 b" N# D/ T
59.   TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
    % t' l1 u8 \/ G5 e1 l
60.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
    
61.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
    
62.   TIM_BDTRInitStructure.TIM_DeadTime = 0;
    5 ]2 c$ @; _; n( M( Q! Y; Q/ |
63.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
    5 v, C# v$ K3 I
64.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;
    
65.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
    6 p9 l) E7 V! h0 p( S  h+ y, M
66.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);
    + a! i) ]" q3 m9 a6 p( F& r6 W
67. 
    # q& L9 P; |  g# k. y
68.   TIM_Cmd(TIM8,ENABLE);
    & O4 i: f/ l: y! ~
69.   TIM_CCPreloadControl(TIM8,ENABLE);
    4 h, A1 |- Z7 _0 _
70.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
    
71.                      
    
72. }</font>

复制代码

编码器初始化函数：
6 r; O' H* O7 c6 j: \* Z- g

1. <font face="微软雅黑" size="3">
   , @. ?1 C" S: I# m- o  s
2. void Encoder_Init_TIM4(void)
   
3. {
   
4.     GPIO_InitTypeDef         GPIO_InitStructure;
   / {  g* r9 ~+ n  o( _: p
5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   
6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
   0 c$ Z1 f" B4 R, T* o, N8 q2 x6 ~
7.     NVIC_InitTypeDef NVIC_InitStructure;
   
8. 
   7 Q; l/ B6 W5 j. A& V
9.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
   8 h3 z" z3 z- O# \. `
10.     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟
    
11.   
    . v4 l0 Y5 z) g. `% V7 G" b, j
12.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用
    % E/ B( W0 d- v. Z5 q% A% n/ ?/ G
13.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用
    
14. 
    4 ]% _4 \7 M  e% \4 ~
15.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
    
16.     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
    
17.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    , q+ L( f, ~+ N$ K8 m* P# l
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.    
    % g0 [3 K" o7 j; u! M! W
23.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    ( j6 S8 o, d# H) T" e
24.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
    - m2 c& P( q; k6 t
25.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    0 h3 b& ?7 C  h9 C* T4 B% ~4 |% p; N
26.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断，防止计数溢出而没有相应函数，造成卡死
    
27.     NVIC_Init(&NVIC_InitStructure);
    # L5 [1 q+ v: G  o; P. I. Q& u5 {/ z
28. 
    & z, ]  B* Y1 p3 b
29. 
    ! z# [; g2 u  F. L8 X9 R) q
30. 
    
31.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
    
32.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
    + J: c/ `% S. M& R1 b5 F
33.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
    + o/ j; W! ]& S8 y
34.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    & N- w) Y  P$ D) r4 Q/ P4 S
35.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    
36. 
    : P7 ]9 e/ `6 N0 l% K4 U) B) c
37.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
    ! D# k$ x( B) }
38.     TIM_ICStructInit(&TIM_ICInitStructure);
    8 I4 z- ]: n$ k! J
39.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
    # O7 r! D$ W: |2 B/ {4 Z/ g
40.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
    
41.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
    
42.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
    % T8 g" ^& i' v% Z) D( `# ^
43.     TIM4->CNT = 0;
    + |1 b; c8 ]' Y: W; n# ~& n0 q& ^
44.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
    
45. }
    8 ]( b6 y0 {" c& }6 a
46. </font>

复制代码

编码器返回速度值：

1. <font face="微软雅黑" size="3">/**************************************************************************
   * [& D+ _  s- X& R6 o; I
2. 函数功能：单位时间读取编码器计数
   
3. 入口参数：定时器
   0 J, J7 u! X# Q2 u; X& M2 `
4. 返回  值：速度值
   5 c  I& B; Z1 X. t& Y) L# B
5. **************************************************************************/
   
6. float Read_Encoder_Speed(uint8_t TIMX)
   
7. {
   8 W* Q( l: @& z
8.     int32_t Encoder_TIM;
   4 i; r4 x7 r* Z; i6 `, N2 U
9.     float res = 0;
   4 `( h0 n  \! |$ Y2 W5 X& G9 }
10.     switch (TIMX)
    9 n" W1 {3 C3 ~2 I4 ]1 [. I' x6 u1 `
11.     {
    & ]1 X/ E% J$ l
12.     case 5:
    4 M' V8 x2 ~4 v1 P1 F
13.         Encoder_TIM = TIM_GetCounter(TIM5);
    
14.         TIM5->CNT = ENCODER_BASE_COUNT;
      ^( C7 m  ^: ?! x1 s
15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    
16.         break;
    
17.     case 4:
    
18.         Encoder_TIM = TIM_GetCounter(TIM4);
    4 S& A/ b9 a$ N
19.         TIM4->CNT = ENCODER_BASE_COUNT;
    % F2 g! ?5 h# C% z* l) v
20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    ' M' X% n2 A+ b7 x0 A  L; F& t0 C
21.         break;
    
22.     default:
    
23.         Encoder_TIM = 0;
    2 f0 \, y1 E+ E  m
24.         res = 0;
    ) f- h) e. |! C
25.     }
    # u' L+ j. g6 x& K2 Q: F5 [1 p
26.     if(res>2048.0f)
    
27.         res-=4096.0f;
    - s- J" [6 x' E
28.     return res*360.0f/4096.0f;
    
29. }
    
30. </font>

复制代码

定时从编码器取数，注意，时间不一样，取回的数值也不一样，取决于实际速度以及编码器线数。这里50ms取一次：
* P2 h' N8 R" ]/ B2 G$ C& p

1. <font face="微软雅黑" size="3">
   
2. void TIM7_IRQHandler(void)//频率20HZ，用于编码器计数
   
3. {  
   
4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   8 s  F3 u0 O9 T# A; @. L
6.     speed=Read_Encoder_Speed(4);
   3 J: w/ c$ j  T
7.   }
   3 S: f) I: k6 p9 ?8 T; F, C' V
8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
   ! Y0 X6 G: H+ f+ E
9. }</font>

复制代码

2.5 PID控制
* W5 |7 H+ a; k, z$ `3 E* e
1 y  \2 |2 x6 o! F6 w( T
PID库函数：
4 y9 q6 K9 D) e/ K; Y% B
8 K' ~! U2 G) F! E1 Z/ z

1. <font face="微软雅黑" size="3">
   
2. #define N 2        //需要对多少变量进行pid调节
   
3. 
   
4. const float KP[N]={1.3,1.0};//这里只用了比例调节
   
5. const float KI[N]={0,0};
   3 `! V; h! d% @- Z: C
6. const float KD[N]={0,0};
   
7. 
   
8. 
   
9. struct _pid{
   8 K4 v2 u1 x! ]+ H
10.   float SetVol;        //定义设定值
    $ \) p; x0 y4 A$ f2 T7 F
11.   float ActVol;        //定义实际值
    - T0 O# t7 M2 o- X, B9 U  W
12.   float Err;          //定义误差
    ! A6 W/ r" r; S7 I( x
13.   float  Err_Next;      //定义上一个误差
    , e. m! B8 ~6 i( @3 |8 i
14.   float  Err_Last;      //定义上上一个误差
    
15.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
    
16.   float integral;      //定义积分值
    $ c0 ]+ M# l( e: }; t, `
17.   float actuator;      //定义控制器执行变量
    
18. }pid[N];
    $ k( n2 n  `8 i! m: _7 \4 x
19. 
    
20. void PID_Init(void)
    . }, @6 L* ^1 d9 b
21. {
    
22.   for(int i=0;i<N;i++)
    
23.   {
    
24.     pid[i].SetVol=0.0;
    " X0 e- Y9 P9 \8 x* d
25.     pid[i].ActVol=0.0;
    
26.     pid[i].Err=0.0;
    6 x2 h, ?( S5 ?: |  z3 x- a- i
27.     pid[i].Err_Next=0.0;
    . a. I* T5 n7 z; q& O
28.     pid[i].Err_Last=0.0;
    
29.     pid[i].integral=0.0;
    6 {  l& b; c0 i0 w' ]) g( V5 Y. u
30.     pid[i].actuator=0.0;
    
31.     pid[i].Kp=KP[i];
    % r1 B& Y# F6 b
32.     pid[i].Ki=KI[i];
    2 U. P" C0 }9 [% T
33.     pid[i].Kd=KD[i];
    ; ]3 N5 `! A. \$ M& S% @6 d) B- F
34.   }
    
35. }
    * u' o: T0 d- w3 ]* y+ G
36. 
    3 [$ A5 Q5 w/ J4 `4 D) @
37. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
    
38. {
    6 f3 i9 _6 N6 }
39.   pid[i].SetVol=set_val;
    . y+ T& F# g9 r0 I# o2 t1 t
40.   pid[i].ActVol=get_val;
    
41.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
    ) {) `3 C- s$ P; a2 q
42.   float IncVol;    //定义增量
    ' X3 s2 b: N1 G) F  i$ h0 w3 H4 c
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);
    9 C& d, h2 H# i* L6 X& z# B( C
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);
    9 r) o$ t& s6 P+ e' e& T, |
46. //  pid[i].actuator=adc_val+IncVol;
    0 B# e- X$ y$ ]( |! L8 }
47.   pid[i].ActVol=pid[i].actuator;
    + n2 L$ h5 y* t' ?2 i  H3 S' ^
48.   pid[i].Err_Last=pid[i].Err_Next;
    
49.   pid[i].Err_Next=pid[i].Err;
    
50.   
    . z( ~3 X0 D; N% t* m6 E
51.   return pid[i].actuator;
    
52. }</font>

复制代码

主函数中的PID调节：
9 ~" h5 N3 P. ^: E* g1 _' [

1. <font face="微软雅黑" size="3">
   
2.     z_get=data[2];
   ( z# f9 c7 F) ?) u
3.     x_get=data[0];
   : D6 T, e9 y% \& F- H2 c9 k. ?
4.     if(z_get-z_set>0.5||z_get-z_set<-0.5)//电机PID
   7 L0 G+ N' C) Q7 B$ e! u' B3 z
5.     {
   . B* i# v# [8 w* J/ X9 Q8 q
6.       LED1=0;  //调节时灯亮
   6 g2 M6 M: H2 g4 \  k% X+ @
7.       PID_val_motor=PID_realize(z_set,z_get,0);
   
8.       PID_val_motor=PID_val_motor/10.0;
   ( Y( [# m0 Q. j0 P
9.       if(PID_val_motor<=0)
   
10.         motor_flag=0;//motor_flag控制电机正反转，PID_val_motor用于改变占空比，范围0~1
    
11.       if(PID_val_motor>0)
    6 X/ A4 e: u6 i  C2 t4 u
12.         motor_flag=1;
    
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;
    " Z. c; g  M+ d7 _' |. M
17.     }
    + q- q  y8 n# W! Q; d* J% o- v
18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
    ' e1 |6 w  q4 P" `% U! V
19.     {
    " ~: U7 t5 B* E" v
20.       LED1=0;  
    / _/ n% b5 y5 n* r  k0 ?' {
21.       PID_val_servo=PID_realize(x_set,x_get,1);
    : X) y1 \- A& Y! t
22.       servo_angle=((140-35)/6)*PID_val_servo+35;//线性映射，把PID的值转化为角度35~140的舵机转角
    : A' D- k! h0 q/ t7 c4 `0 H3 _
23.       if(servo_angle<35)servo_angle=35;
    
24.       if(servo_angle>140)servo_angle=140;
    1 R! \" f/ ?- T  z9 N. |" @( M9 q
25.     }
    
26.     LED1=1;</font>

复制代码

定时器TIM2中断里改变占空比：
! L8 W* q4 g+ d& G, ]: m2 B+ A, Q

1. <font face="微软雅黑" size="3">
   
2. void TIM2_IRQHandler(void)
   
3. {  
   . q! B+ P# S* `% x- ^! E% X+ }
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   ' I1 j5 m8 n0 _
5.   {
   
6.     if(motor_flag==1)//反转
   
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   
9.       TIM_SetCompare2(TIM8,0);
   4 I6 `7 c. R! c. ?; w2 {6 h( i
10.     }
    
11.     if(motor_flag==0)//正转
    
12.     {
    
13.       TIM_SetCompare1(TIM8,0);
    & _8 n1 U# G; P8 j' ~- z
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    
15.     }
    9 }8 {; O/ r/ `
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，根据舵机手册得到占空比与转角的关系，引脚：PA6
    $ N9 ~) |) r. d2 Z3 f+ O" p
17.   }
    ; Z" k, Z6 u! [  m! m
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    * _6 ^) o5 e) w. A+ @7 C
19. }
    
20. </font>

复制代码

* J5 I* ]8 t8 D9 X* Q7 M! p
1 Y" Z. [; E9 G$ G1 S: ?% r

优秀
# ^) ^& c5 o* y$ u( L: ]