概述
小车外形：
! s3 m2 c7 \) r# v
" F5 _/ @% A$ x7 C1 W1 R9 C5 b

( F9 _# ]  k& m# c/ U  D3 m! `- S+ W功能简介
利用摄像头识别前车尾部的AprilTag，得到前车位置，传回stm32主控板处理，使两车在行驶时保持恒定距离，实现自动跟车。
/ q5 c5 y# v) Y5 S9 `
) X. h$ p0 G3 Z4 U4 z9 E
+ s5 P4 M  U8 _1 p# H

0 S1 x: U" }$ RopenMV4摄像头
; Y* G& Y* U( d5 [' b4 W7 d1.1 Apriltag识别与串口传输

3 F' F: R2 ]. Q& k  J) w
AprilTag是一个视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志（与二维码相似，但是降低了复杂度以满足实时性要求），可以快速地检测标志，并计算相对位置。

) v1 h$ h0 ~# i) U& q- }1 q) I
1 T7 X$ S; O  q/ ]5 JApriltag示例：

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

1. <font face="微软雅黑" size="3">
   
2. import sensor, image, time, math,pyb
   ) t3 f1 v$ z& D* d; f9 f+ K
3. from pyb import UART
   0 _! u) }. G0 I/ U( |
4. 
   
5. sensor.reset()
   
6. sensor.set_pixformat(sensor.RGB565)
   " R; F2 l$ ^& [! ?) Q2 z
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...
   ( J; _0 w$ @6 B. Z
10. sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
    8 I: E8 G. B" P. w8 s+ a
11. clock = time.clock()
    
12. uart = UART(3, 115200)#串口波特率
    . e1 a  C& W2 z- y9 }* f: l
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
    5 F: c; b+ ~/ p# P) P
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)
    ) A; j" |/ o  J0 z! i+ U
18. 
    
19. def degrees(radians):
    
20.     return (180 * radians) / math.pi
    
21. 
    
22. while(True):
    ) b7 d: j! g% G% p3 @
23.     clock.tick()
    7 @: t) U7 z, k1 _& R  y$ a5 q2 V
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
    ( Y' T9 v; [6 {" U; a8 n; }
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())
    
29.             #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
    
30.         # Translation units are unknown. Rotation units are in degrees.
    " t2 ], L9 f4 h$ r& z' 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分割取得数据
    
33.         #pyb.delay(500)
    
34.    # print(clock.fps())</font>

复制代码

STM32主控板（型号为F407）
! S2 G- F  S9 ^" F3 _5 l

) A+ `" D2 y' t2.1 时钟与中断配置


附上stm32时钟示意图：
7 A! J& B* n/ D: A  _- \1 h
* a2 J+ S6 u1 [% R定时器示意图：

( g& C0 g8 {4 S4 N定时器分配：
7 g6 P" Z- c, x/ y5 y8 r7 ?- j" _! j  ^& d
! `# b7 J, U$ a: c4 M) @3 P; B所有时钟初始化的函数：（每个函数的详细内容在后面）

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
   
4.   TIM2_Int_Init(400-1,20-1);//定时中断，21KHZ
   4 J! L4 j$ H8 _! u$ M. s& @
5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数，20HZ，50ms中断一次
   ) o1 s" C2 L4 f& [  ^4 d; [! V
6.   uart_init(115200);  //初始化串口1波特率为115200
   
7.   Encoder_Init_TIM4();//编码器接口初始化</font>

复制代码

2.2 串口收发与数据处理
' Z! e6 }* R2 e; x; d: l9 _

串口中断：USART1，USART2
串口初始化函数（以USART1为例）：
6 X; ?$ o4 t$ P9 Z3 b$ B' |4 I5 N' K

1. <font face="微软雅黑" size="3">
   
2. void uart_init(u32 bound){
   
3.    //GPIO端口设置
   
4.   GPIO_InitTypeDef GPIO_InitStructure;
   
5.   USART_InitTypeDef USART_InitStructure;
   ' e" i. Q8 }6 `+ w6 ^7 A5 S
6.   NVIC_InitTypeDef NVIC_InitStructure;
   4 J" }# r" d8 Y; ^0 V5 N" W
7.   
   
8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
   ; K# a0 o! D4 I' d/ g6 \
9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
   8 s$ v- @9 Y7 |! s9 T
10. 
    + a9 S" f2 _/ x( v1 d
11.   //串口1对应引脚复用映射
    4 O+ g# G1 {9 t
12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
    - w( o$ Y3 v& |2 \/ t
13.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
    
14.   
    
15.   //USART1端口配置
    
16.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
    7 ?$ U& M* z2 x7 g
17.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    # |5 Y2 b9 P+ j
18.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度50MHz
      @  s, m& E+ O0 k2 L% P! l# E
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. 
    , d3 @" [( s8 S, T1 M% i5 R
23.    //USART1 初始化设置
    / W% B) Y+ ?7 Y
24.   USART_InitStructure.USART_BaudRate = bound;//波特率设置
    
25.   USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
    & N. k/ Q# M8 L
26.   USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
    
27.   USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
    5 m2 c8 F" g% d& p! u) X
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
    
31.   
    
32.   USART_Cmd(USART1, ENABLE);  //使能串口1
    6 P7 x6 S' j3 {' M$ r' O$ D1 _
33.   
    0 k' c0 g% @+ [  Z1 y1 q
34.   USART_ClearFlag(USART1, USART_FLAG_TC);
    
35.   
    
36. #if EN_USART1_RX  
    9 @9 i* b' I' Y) d/ j: {" p0 u% A) O
37.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
    3 [# ~: I' y  r: T( w' Q
38. 
    0 u1 b& {. V7 M& I
39.   //Usart1 NVIC 配置
    
40.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
    3 M( X2 w9 _* k
41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
    
42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
    # p5 r; |1 H5 \) e# |* r
43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
    
44.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、
    1 F: N8 `9 N! Y8 P
45. 
      U  s9 @8 e4 ]( w
46. #endif
    8 a6 W6 M3 z. b% k* V; H
47.   
      N0 |# e% U( t1 `
48. }</font>

复制代码

串口中断处理函数：

1. <font face="微软雅黑" size="3">
   % M0 Y; b) }7 y( j
2. void USART1_IRQHandler(void)                  //串口1中断服务程序
   
3. {
   
4.   u8 Res;
   " j5 V( F5 b% X% ~, e' ^8 j
5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
   
6.   OSIntEnter();   
   
7. #endif
   
8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
   
9.   {
   . J- N" a& ]* v
10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
    
11.    
    
12.     if((USART_RX_STA&0x8000)==0)//接收未完成
    1 K' @/ F' S) c
13.     {
    ; s* h' r  Y6 t# n- k% T+ G
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
    
23.         {
    
24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
    % ]& q9 U# m# i. o: T# @
25.           USART_RX_STA++;
    " z2 ?' }3 \6 V$ g
26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
      r8 [7 @4 U7 |0 L
27.         }     
    : f3 u0 C+ |* f$ ?7 }
28.       }
    
29.     }        
    
30.   }
    " @, ^  h) n% Y
31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
    6 U* `# W+ d4 B' S8 E4 r3 w' n4 {( ~$ I
32.   OSIntExit();                        
    " \& i8 Z7 `; \. [+ E: d
33. #endif
    ! m. y" F/ j* j# N3 [6 X, U1 O
34. }
    3 u5 s! {: i! _; b$ O4 ~
35. #endif  </font>

复制代码

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

1. <font face="微软雅黑" size="3">/*涉及到的全局变量
     f0 Z% r7 A( s, t: ^4 k, x
2. float data[3];//x,y,z方向的距离，浮点数形式
   
3. unsigned char data_string[3][7];//x,y,z方向的距离，字符串形式
   
4. */
   
5. if(USART_RX_STA&0x8000)
   ' n) a% m' F: z6 k
6.     {  
   / _0 z3 d* K. f3 E4 ?
7.       //清空字符串
   
8.       for(i=0;i<2;i++)
   . V5 X; C( K( h) N
9.       {
   8 }$ o2 J  T: R$ R# m/ T; T2 @
10.         for(j=0;j<6;j++)
    ) B+ _0 y% C" K1 Q: x) w* U1 v
11.         {
    
12.           data_string[i][j]=' ';
    
13.         }
    + M. n* M8 `2 G2 S- v
14.       }
    
15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
    & c# l+ \" I8 V; B" V
16.       for(t=0,j=0;t<len;t++)
    
17.       {
    / \7 W. x$ u, z( ]1 l8 c
18.         j=0;
      x' T1 }' O. W- o; U
19.         if(USART_RX_BUF[t]=='A')
    
20.         {
    
21.           t++;//去掉首字母
    $ p: i% I! J1 r8 g3 w. ?/ X
22.           while(USART_RX_BUF[t]!=',')
    6 B( P6 M9 W! A
23.           {
    - x8 o. n: @% I, a7 }( C
24.             data_string[0][j]=USART_RX_BUF[t];
    , x6 u5 b( U' {0 }
25.             t++;
    & n( `6 Q3 v' o1 D
26.             j++;
    " C( M& l+ \' m2 X/ d
27.           }
      h0 v. j8 k, X, E0 I  w8 p$ `
28.         }
    ! X' h0 U/ o7 M. B
29.         if(USART_RX_BUF[t]=='B')
    % M+ t7 L4 Q& v6 ~% Z" S* r
30.         {
    
31.           t++;//去掉首字母
    
32.           while(USART_RX_BUF[t]!=',')
    
33.           {
    
34.             data_string[1][j]=USART_RX_BUF[t];
    
35.             t++;
    
36.             j++;
    
37.           }
    
38.         }
    
39.         if(USART_RX_BUF[t]=='C')
    2 E4 k5 o& t; p8 w+ A
40.         {
    
41.           t++;//去掉首字母
    ( f% @; T5 j: N1 T8 l* V% m
42.           while(USART_RX_BUF[t]!=',')
    
43.           {
    
44.             data_string[2][j]=USART_RX_BUF[t];
    0 S# y; U, y  p1 ]* ^8 D- x& F( y
45.             t++;
    $ D8 \4 E( b4 J/ t0 E- {
46.             j++;
    / o% c" S3 H8 F, C% R
47.           }
    % i/ `- z% `7 W$ t7 e5 d- s
48.         }
    
49.       }
    
50.       //把字符串转化为浮点数
    ' F9 E# ?% m1 Y( ]* _8 R. N
51.       data[0]=myatof(data_string[0])/100.0;//x
    
52.       data[1]=myatof(data_string[1])/100.0;//y
    
53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z，输入是负数，转换为正
    8 v$ r9 x: t! \8 m& j! t0 b
54.         //USART2发送数据
    % |- R  B# W. E; d6 @
55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
    
56.         
    + ]; z  I6 s+ C2 d
57.       //LCD更新显示
    
58.       //显示xyz
    
59. //        CLEAR(10);
    1 _2 d$ v1 L/ W9 `1 m1 a' j6 Y
60. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);
    2 e- f+ a$ e/ ~& {# U" V0 d: w
61. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,data_string[0]);
    
62. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,clear);
    5 [9 i( U) J# D0 M5 p& K$ d* `
63. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
    
64. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
    . }) C+ M- d  K- f, N
65. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);
    
66.         
    + {0 m2 H* `+ K; v8 O
67.         USART_RX_STA=0;//清除标志位
    - u$ ^$ b; C- ?% t. G$ N
68.     }
    
69. }
    / x6 ~5 h2 Y0 v6 a) Q; L
70. </font>

复制代码

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

; c0 O* B; b3 W  F

1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数，返回的是乘100后的int值，因float返回有错误
   
2. {
   
3.   int flag = 1;//表示正数
   
4.   int res =0;
   5 R8 o' E; n/ N% g! v# s
5.   u8 i=1; //小数点后两位
   
6.   while(*str != '\0')
   " Q( `5 s1 f# A& L/ V& T
7.     {
   " k. w  b$ G" |# _! k* g
8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
   
9.       {
   1 f- t4 g  k9 |$ e* t- k
10.         str++;
    7 O! ^. h/ `; y0 V& B8 X" B
11.         continue;
    $ h- W! e) q% K6 X
12.       }
    . k; A& Y, T  v% y- x! N
13.       if(*(str-1) == '-')
    
14.       {
    
15.         flag=-1;//表示是一个负数
    
16.       }
    
17. 
    
18.     while(*str >= '0' && *str <= '9')
    ( d- g/ J5 ?! p$ l  F. r4 w
19.     {
    2 }/ t& m& k; ~, {! y
20.       res = res *10 + (*str - '0');
    9 Z8 F: A( Y: y, }1 R
21.       str++;
    . Q% z3 B, v9 n+ H  ^6 M
22.     }
    
23.     if(*str == '.')
    ' j. Y; `9 i: s, q; C2 n; a/ Z. m) R
24.     {
    
25.       str++;
    
26.       res = res *10 + (*str - '0');
    
27.       str++;
    : Y8 N6 y0 ?) B
28.       res = res *10 + (*str - '0');//保留两位，故加两次
    
29.       return res*flag;
    ! u# \* t2 i$ W4 A, O- x. `3 t
30.     }
      e, v5 y4 G/ A. v& U; u
31.     }
    
32. }
    & ^4 d! f# r( s' H
33. 
    0 P8 t9 p: L# J& I- o) C& M
34. 
    $ p. A9 S* T& e" o( H/ j
35. 2.3 LCD显示模块
    
36. 
    
37. LCD模块用于调试时观察数据，调试完成可以删去，因为显示屏很耗时，使处理速度变慢
    
38. 驱动函数总览：
    
39. 
    
40. 
    ) ^  r5 S+ h4 i' C
41. 
    . y, v1 q( {5 B8 v
42. void LCD_GPIO_Init(void);
    , j# \! X. I) a# e$ w. ]0 @
43. void Lcd_WriteIndex(u8 Index);
    
44. void Lcd_WriteData(u8 Data);
    
45. void Lcd_WriteReg(u8 Index,u8 Data);
    - V7 d+ S* v6 G' h+ z
46. u16 Lcd_ReadReg(u8 LCD_Reg);
    
47. void Lcd_Reset(void);
    & G2 ^) C6 L. [( E
48. void Lcd_Init(void);
    " z0 @2 ]3 Y8 C1 @! Y- ?
49. void Lcd_Clear(u16 Color);
    / H% A) j; A% P
50. void Lcd_SetXY(u16 x,u16 y);
    3 r: N3 e" I' r1 r  o
51. void Gui_DrawPoint(u16 x,u16 y,u16 Data);
    % z- \. |# {* F( ?5 N, e
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);
    . Y' J& X8 h: w1 |
54. void LCD_WriteData_16Bit(u16 Data);</font>

复制代码

TFT屏幕初始化：
2 G  \( y# u8 V) C$ U; Y

1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
   
2. {
   $ C! K5 R# L+ R2 h
3.   Lcd_Clear(WHITE);
   , i/ ?# j4 A: H. A! q6 b. [" j
4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
   
5.   delay_ms(1000);
   
6.   Lcd_Clear(WHITE);
     o1 E3 A+ [$ d, x4 v
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">
   : n2 C8 F1 O8 B6 j+ f8 {- f
2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
   7 q; D( o; p3 c, Z2 u$ Z
3. {
   2 B$ y- F$ e3 w/ Y) Q7 z1 u% O' v
4.   unsigned char i,j;
   5 j. F5 ^  x' j+ L  m& N
5.   unsigned short k,x0;
   8 g; J8 U/ z- M+ W! _7 k3 Y3 ]
6.   x0=x;
   
7. 
   8 R. R8 }2 Y9 V* K
8.   while(*s)
   + V1 @4 z, A, s: c% S/ {1 Q. n2 x1 e
9.   {  
   # `  @- E; d+ `% s
10.     if((*s) < 128)
    % i5 Y' w5 m' }0 D/ R
11.     {
    
12.       k=*s;
    5 X5 K1 O4 H2 s0 y* m+ H, Q3 G; ^
13.       if (k==13)
    
14.       {
    
15.         x=x0;
    " J( }8 {# i; A" G2 j
16.         y+=16;
    
17.       }
    
18.       else
    # R3 }: N3 G/ E2 [& z
19.       {
    . I6 J( ^6 L. R
20.         if (k>32) k-=32; else k=0;
    
21.   
    ! R( c+ [& f8 z; W  @
22.           for(i=0;i<16;i++)
    
23.         for(j=0;j<8;j++)
    ( ]) M3 Q- `  q' B  T" D+ C
24.           {
    
25.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
26.             else
    3 O9 T: y8 Z# ^7 u/ `; }
27.             {
    
28.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
29.             }
    
30.           }
    1 V6 K: B/ ]. P
31.         x+=8;
    3 f+ K2 G/ }+ ^. o  D
32.       }
    4 R% ~( E9 N0 v4 O+ |5 o5 ^
33.       s++;
    1 c" o& K# {1 p0 J
34.     }
    
35.       
    
36.     else
    
37.     {
    
38.    
    , v$ g; C7 G% z8 d' b( s- U- h+ v
39. 
    
40.       for (k=0;k<hz16_num;k++)
    ) b& l% `4 Y# J! N
41.       {
    . n* T" `2 c% P, \
42.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
    
43.         {
    6 m1 x+ A8 p9 ^) e) w& ?" C
44.             for(i=0;i<16;i++)
    7 _* o  z+ h+ T3 F7 e0 l
45.             {
    - X# p9 g; O+ T1 \) h/ f+ |/ ]/ Q* k! d
46.             for(j=0;j<8;j++)
    
47.               {
      |) j' x: {5 s1 r& W% q* Y
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);
    
51.                 }
    
52.               }
    
53.             for(j=0;j<8;j++)
    / V- S# G' p/ Z% t$ r% L5 R4 Y
54.               {
    ) C( w6 O+ v9 ]5 U9 V7 N: W
55.                   if(hz16[k].Msk[i*2+1]&(0x80>>j))  Gui_DrawPoint(x+j+8,y+i,fc);
    * ~3 D+ ^' k+ Y" g) K% D, M
56.                 else
    2 H3 t5 \& W5 o- z' e4 d6 M
57.                 {
    
58.                   if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);
    ! E7 l. ?+ k- h7 Q
59.                 }
    7 c1 H, V3 [- d
60.               }
    
61.             }
    
62.         }
    
63.         }
    
64.       s+=2;x+=16;
    8 h+ l* k, `) w8 O  b0 v
65.     }
    / l( X1 k4 ~6 X6 X& ?
66.    
    
67.   }
    
68. }
    
69. </font>

复制代码

2.4 电机、舵机与编码器


定时中断：TIM2，用于修改电机和舵机的PWM占空比
: u. K# ]7 p2 V初始化函数：
, x5 o" r% t. L7 P$ s% ~7 w- U
3 c/ E" r7 P- ~4 n

1. <font face="微软雅黑" size="3">
   3 h  s( D" |1 c, U& e
2. //通用定时器2中断初始化
   . @/ [, v$ Y9 k( p% ^
3. //arr：自动重装值。
   
4. //psc：时钟预分频数
   2 R. G" C7 Q5 M$ v! `
5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
   : ~2 p1 @7 s! w9 R5 N( ^! j
6. //Ft=定时器工作频率,单位:Mhz
   - o2 {" X- j. }4 ?% [, |
7. //这里使用的是定时器2!
   $ I- z" v& m. f- R6 K4 \
8. void TIM2_Int_Init(u16 arr,u16 psc)
   
9. {
   
10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    
11.   NVIC_InitTypeDef NVIC_InitStructure;
    ! D8 ^) `' Y" ^/ n
12.   
    * X# R3 O: q7 |: @) }1 E7 o
13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
    9 d  v4 L' U8 H0 |- K/ G
14.   
    . P8 @/ a+ K6 Q! j% m# W5 M
15.   TIM_TimeBaseInitStructure.TIM_Period = arr;   //自动重装载值
    
16.   TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    
17.   TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
18.   TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    6 m# ^5 x0 v- P; \
19.   
    4 V6 g( q- K7 i4 F0 L5 H: {3 s  `8 ]
20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
    
21.   
    
22.   TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断
    
23.   TIM_Cmd(TIM2,ENABLE); //使能定时器2
    
24.   
    
25.   NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器2中断
    + P& Y! {0 t% H1 d
26.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级1
    
27.   NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
    3 @: F) _- M+ l+ R; I# U2 i( Q
28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    
29.   NVIC_Init(&NVIC_InitStructure);
    6 U; n3 L" X9 N
30. 
    4 p( l& p5 @. B
31. }</font>

复制代码

TIM2中断处理函数：

1. <font face="微软雅黑" size="3">
     _4 \: P1 g& S# M# q  q& U+ i  r
2. void TIM2_IRQHandler(void)
   % Z. O: C$ t6 ]+ l
3. {  
   & A: }! Y  \  \3 G! P$ A1 K. h
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   
6.     if(motor_flag==1)//反转
   . X9 P, h0 J  V5 _5 C
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   
9.       TIM_SetCompare2(TIM8,0);
   
10.     }
    
11.     if(motor_flag==0)//正转
    
12.     {
    
13.       TIM_SetCompare1(TIM8,0);
    & W3 E5 `, L0 [7 t  w5 q
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
    6 j# G$ m$ D# ~, q7 d# ~5 @6 q! T
17.   }
    
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    
20. </font>

复制代码

PWM输出：TIM3（舵机），TIM8（电机）
初始化函数（以TIM8为例）：
5 F7 o0 O  m4 `9 F% n1 L$ s
+ I1 g# m! C( |$ n

1. <font face="微软雅黑" size="3">
   ; a9 A5 w3 M+ y2 Q6 M- r& Q
2. void TIM8_PWM_Init(u32 arr,u32 psc)
   
3. {               
   
4.   //此部分需手动修改IO口设置
   2 j& ~$ c5 c* ]# }* T
5.   
   
6.   GPIO_InitTypeDef GPIO_InitStructure;
   3 Z% K+ j; o; w5 k
7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   ) s6 y* G. L: ?; E4 T+ @4 [1 e
8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
   ( K# l# E6 E2 w+ l" n  k
9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
   # T% z+ r6 V* D2 U
10.   
    
11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
    
12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
    
13.   
    + F) g8 O7 Q% r
14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
    
15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    ! b* B* c7 ~2 e$ ^
16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
    2 c; y6 r) {: i2 d4 n) `
17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    8 M3 C: e/ a/ d9 ?9 q1 a
18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
    
19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7
    + _# v5 f+ z) o  T* _  `
20. 
    
21.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    7 ?$ }# x' w) u: W8 J
22.   GPIO_Init(GPIOB,&GPIO_InitStructure);
    * p) q9 z, M+ r3 |* v
23.   
    " L% \3 p+ O$ E) a( k8 a$ w0 |
24.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
    : B% B% r: n; Y) Q
25.   GPIO_Init(GPIOC,&GPIO_InitStructure);
    
26.   
    7 b' T' Q1 E8 k3 w% f  Z- h5 _. @
27.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
    2 G% d  @/ Z  x
28.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1
    8 `# Z& L' |: q3 A; I* M# Y
29.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
    5 c* \  p+ N/ j# I% f
30.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13复用为定时器1
    
31.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14复用为定时器1
    
32.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15复用为定时器1
    " |6 T: S' {% S; A; b
33. 
    / ]' Z2 c+ L, A1 [8 d  F  G* F
34.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
    
35.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    5 L  Y5 ^0 ]# V; B' F6 u
36.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
    
37.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
38.   
    ) Z7 {% I4 T1 S/ b# @4 y# w( E
39.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
    
40.   
    % L8 z! d, _( O# o( ^
41.   //初始化PWM模式   
    + N5 y) ?7 l. q; v
42.   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    : {! ]" U; y% L4 p! t
43.   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    & F. l0 x) }* H8 R% e/ c) {
44.   TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    
45.   TIM_OCInitStructure.TIM_Pulse = 0;
    
46.   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    
47.   TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
    ' x% `& @2 g* K* E# C
48.   TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
    . J5 k% K1 ]1 V/ r
49.   TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
    - o9 p: a7 D4 h; W( B; P
50. 
    + I5 D. x# b# B% l, h. J
51.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    7 A& }* t+ ]" e+ C6 ~3 M
52.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    % a5 k% X% Z+ ~6 x+ a- M
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);
    - q) h  d9 z7 U. I% a4 X! o
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;
    
65.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
    
66.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);
    3 W4 v/ V" \% k" r
67. 
    
68.   TIM_Cmd(TIM8,ENABLE);
    
69.   TIM_CCPreloadControl(TIM8,ENABLE);
    4 ~9 i, G1 t" ~1 ?+ [* C. u8 j3 G
70.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
    ; Z" i' p0 x+ Z$ ]: T0 N  R
71.                      
    . n- M& d. g& z7 `* p
72. }</font>

复制代码

编码器初始化函数：

1. <font face="微软雅黑" size="3">
   
2. void Encoder_Init_TIM4(void)
   0 X8 e4 `- e& _
3. {
   9 u3 ?# p: }, H4 ^
4.     GPIO_InitTypeDef         GPIO_InitStructure;
   
5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   & M- e% c2 Z: I$ x% l6 w( ?
6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
   ) d* X3 s  |6 f" ^5 k  D
7.     NVIC_InitTypeDef NVIC_InitStructure;
   9 X- `! T% q$ s/ w
8. 
   3 z' b3 W3 v! M
9.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
   
10.     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟
    
11.   
    9 p0 l  f! `9 h* B* L9 W
12.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用
      ~0 o$ a8 y  B& m* i5 z9 D3 S3 Y2 D0 Q
13.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用
    9 d; a4 o5 i4 x9 `7 n7 U  l
14. 
    
15.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
    
16.     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
      J8 z% u2 M0 \0 K/ k
17.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    8 x  F3 x) [( j! l3 S2 _4 j8 b
18.     GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    7 N! e% z. D( U; }/ R! K4 [
19.     //GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL ;
    3 p  L8 Z9 D- k/ \% P) h2 y
20.     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
    , Z1 L0 U6 {3 I5 L/ a. f+ ~+ B
21.     GPIO_Init(GPIOB,&GPIO_InitStructure);
    4 {+ r5 M+ d! \* n- f
22.    
    
23.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    6 F/ [5 i" K0 K* w
24.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
    
25.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    
26.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断，防止计数溢出而没有相应函数，造成卡死
    2 H- Q8 R5 |: b3 K$ ?  I
27.     NVIC_Init(&NVIC_InitStructure);
      \, j. q' \* D, d' h2 W& Z3 A
28. 
    
29. 
    
30. 
    4 P/ x4 O. B0 U, K$ ~7 H
31.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
    
32.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
    5 D1 y! S; C' X& [( W0 O, D; o
33.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
    
34.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    & E5 n3 l) c# O7 l: ~9 p" k
35.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    3 Z7 [: z% x( L5 N) C6 B0 M
36. 
    $ }* }8 [  U" b# p; o
37.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
    3 o+ j& p& r+ p9 E8 r( d+ H( W, Q- C
38.     TIM_ICStructInit(&TIM_ICInitStructure);
    $ S% A' X- f# V* a, s, |  F8 J
39.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
    9 O- _. S1 {( l: w* P
40.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
    7 S: _- j1 B1 \* m2 `
41.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
    / G7 o0 E/ Q) c3 H/ K: N) Y  l6 X
42.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
    
43.     TIM4->CNT = 0;
    
44.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
    
45. }
    ( A4 @+ Z' o' N; T* s7 w. S
46. </font>

复制代码

编码器返回速度值：

1. <font face="微软雅黑" size="3">/**************************************************************************
   
2. 函数功能：单位时间读取编码器计数
   
3. 入口参数：定时器
   
4. 返回  值：速度值
   % m! R( O0 u' `. C4 [! Y9 f
5. **************************************************************************/
   
6. float Read_Encoder_Speed(uint8_t TIMX)
   " q9 e7 T1 N" e- s" f/ G
7. {
     A6 }2 `. y' A
8.     int32_t Encoder_TIM;
   
9.     float res = 0;
   
10.     switch (TIMX)
    
11.     {
    
12.     case 5:
    1 |  o% C% Y' W3 o  C
13.         Encoder_TIM = TIM_GetCounter(TIM5);
    
14.         TIM5->CNT = ENCODER_BASE_COUNT;
    6 I9 }% u+ ?% }' y1 Q
15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    
16.         break;
    ( a# d4 A; B9 e2 s1 ]& N5 }
17.     case 4:
    
18.         Encoder_TIM = TIM_GetCounter(TIM4);
    
19.         TIM4->CNT = ENCODER_BASE_COUNT;
    - M8 ?$ r! b" c3 m
20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    & J2 E: x6 M* Z: r" ^$ p4 J
21.         break;
    
22.     default:
    
23.         Encoder_TIM = 0;
    # w, A# `8 [, Y
24.         res = 0;
    
25.     }
    ) T2 Y5 e' V9 H) K' ?
26.     if(res>2048.0f)
    / i  y; e- I2 E
27.         res-=4096.0f;
    - |! [# l" Y2 k( [, s2 G" w" x
28.     return res*360.0f/4096.0f;
    
29. }
    3 m% ^* ]# r$ M; Y8 T
30. </font>

复制代码

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

1. <font face="微软雅黑" size="3">
   
2. void TIM7_IRQHandler(void)//频率20HZ，用于编码器计数
   ' [+ Y! g0 Y; c1 P! Z6 S- N
3. {  
   
4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   
6.     speed=Read_Encoder_Speed(4);
   1 @5 \. |/ T5 ^3 {- }- m
7.   }
   ) z0 x4 k# u2 m  s; S; A' p4 V$ t
8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
   
9. }</font>

复制代码

2.5 PID控制


PID库函数：

8 N1 t" l% }$ H0 F

1. <font face="微软雅黑" size="3">
   " I- {# f. @) u- f& C
2. #define N 2        //需要对多少变量进行pid调节
   3 Z/ E" ^. H( z. W! e0 a5 d$ V' r
3. 
   . s7 Z- U- P& N* L: Y
4. const float KP[N]={1.3,1.0};//这里只用了比例调节
   
5. const float KI[N]={0,0};
   + _4 E/ x/ d' [/ @  X+ }
6. const float KD[N]={0,0};
   
7. 
   3 ?% I2 W5 h! K+ i6 F
8. 
   
9. struct _pid{
   
10.   float SetVol;        //定义设定值
    1 X6 }  ^' a, o
11.   float ActVol;        //定义实际值
    ( }* t% U2 B  N8 X" `( v9 Q- A
12.   float Err;          //定义误差
    0 u  q. V; K7 v7 R
13.   float  Err_Next;      //定义上一个误差
    ) t- y+ K$ [4 H+ P
14.   float  Err_Last;      //定义上上一个误差
    
15.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
    
16.   float integral;      //定义积分值
    
17.   float actuator;      //定义控制器执行变量
    9 `9 i7 ^+ l& B9 T5 ?, g3 {3 x* l
18. }pid[N];
    
19. 
    
20. void PID_Init(void)
    
21. {
    
22.   for(int i=0;i<N;i++)
    - Y/ q; O& j) m1 z! O5 \4 p
23.   {
    + u2 G& |/ f, k
24.     pid[i].SetVol=0.0;
    0 |) W$ H3 R9 i/ \- }5 b9 K! ^
25.     pid[i].ActVol=0.0;
    
26.     pid[i].Err=0.0;
    
27.     pid[i].Err_Next=0.0;
    ) n& Q3 B; a" m
28.     pid[i].Err_Last=0.0;
    
29.     pid[i].integral=0.0;
    / q/ B" D1 x( G) G. R
30.     pid[i].actuator=0.0;
    
31.     pid[i].Kp=KP[i];
    
32.     pid[i].Ki=KI[i];
    
33.     pid[i].Kd=KD[i];
    1 |0 \, ~& J$ Z; K% n
34.   }
    
35. }
    
36. 
    
37. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
    . B& ]+ f: U: s1 a, }1 V. q
38. {
    - W7 g+ G. S' g* Z
39.   pid[i].SetVol=set_val;
    
40.   pid[i].ActVol=get_val;
    
41.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
    ( O# P( O; L- w, {  {
42.   float IncVol;    //定义增量
    & M5 n! q# G' r! z7 J) g5 D+ |5 z6 z
43.   pid[i].integral+=pid[i].Err;
    ( a) \& G( m" g* Z) w5 a5 W' h
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 O6 i7 \3 m: N5 p
46. //  pid[i].actuator=adc_val+IncVol;
    $ v9 T6 T' M- o
47.   pid[i].ActVol=pid[i].actuator;
    
48.   pid[i].Err_Last=pid[i].Err_Next;
    
49.   pid[i].Err_Next=pid[i].Err;
    
50.   
    % z5 c6 P: c: N, ~
51.   return pid[i].actuator;
    
52. }</font>

复制代码

主函数中的PID调节：

% F$ n/ F+ Q7 H. x0 v

1. <font face="微软雅黑" size="3">
   
2.     z_get=data[2];
   
3.     x_get=data[0];
   9 O5 I9 F+ M  g
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);
   # g, a7 e0 Y. ]" E; |
8.       PID_val_motor=PID_val_motor/10.0;
   
9.       if(PID_val_motor<=0)
   7 }# M8 d6 p/ ~% U* X+ u
10.         motor_flag=0;//motor_flag控制电机正反转，PID_val_motor用于改变占空比，范围0~1
    3 \/ y3 x' j2 _
11.       if(PID_val_motor>0)
    
12.         motor_flag=1;
    
13.       PID_val_motor=abs_float(PID_val_motor);
    
14.       if(PID_val_motor>2)PID_val_motor=0;//标志太远，让车停止
    8 z' S7 k- t% U  m% e" U
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;
    9 J, P( C. o- a% }5 f
17.     }
    
18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
    
19.     {
    5 v/ i' e8 ^5 L0 i; n. D
20.       LED1=0;  
    ! _% {( Q% n) r, c
21.       PID_val_servo=PID_realize(x_set,x_get,1);
    - h: q: `% b* \
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.     }
    
26.     LED1=1;</font>

复制代码

定时器TIM2中断里改变占空比：
( f0 p$ ~! S3 E. {

1. <font face="微软雅黑" size="3">
   
2. void TIM2_IRQHandler(void)
   
3. {  
   
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   . E3 `, S) r& [% N/ e1 `* v
6.     if(motor_flag==1)//反转
   
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   ' k9 q9 e, E. v$ F! s% A+ V7 P
9.       TIM_SetCompare2(TIM8,0);
   6 o! `' V" J- G& u
10.     }
    
11.     if(motor_flag==0)//正转
    ' ]% K6 ~% e; i
12.     {
    / G4 I1 _9 V+ n2 Q2 w
13.       TIM_SetCompare1(TIM8,0);
    
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    , J3 W# P0 |8 R( E* s. r
15.     }
    
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，根据舵机手册得到占空比与转角的关系，引脚：PA6
    & M. ~1 o  c/ [0 f, J3 D6 r/ l
17.   }
    $ W) n( ?; U3 \$ x& E% S
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    9 s8 T# R$ M) p4 |! [7 W% u0 H& l  M
19. }
    2 \1 a% r$ ~  y0 W) h. r
20. </font>

复制代码

) A6 p  |2 U- F- |3 p, A

优秀
$ v% F- g  P2 p2 Z9 j; J$ r9 U