概述
. O: N) n/ a$ d7 D" ]) B. f! p小车外形：
7 X' r2 S! l& u6 Y$ h+ m- X$ n
7 \$ O- G; _* i7 c2 M

( _4 |3 h4 G2 t9 o4 e! O/ H功能简介
利用摄像头识别前车尾部的AprilTag，得到前车位置，传回stm32主控板处理，使两车在行驶时保持恒定距离，实现自动跟车。
: C5 d: `- v1 W
+ x; C9 j* F& C  r' g& y8 g
2 V+ O0 }! k( C+ c* w0 K
& D' ~1 E2 @4 f' A1 h
openMV4摄像头
1.1 Apriltag识别与串口传输


7 K. ?! p# l# n8 J7 i3 xAprilTag是一个视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志（与二维码相似，但是降低了复杂度以满足实时性要求），可以快速地检测标志，并计算相对位置。

# \+ ~4 U1 n. s
Apriltag示例：

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

1. <font face="微软雅黑" size="3">
   
2. import sensor, image, time, math,pyb
   
3. from pyb import UART
   
4. 
   
5. sensor.reset()
   
6. sensor.set_pixformat(sensor.RGB565)
   
7. sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
   + Q. G: c" v4 B7 E; r6 b, o) [0 z
8. sensor.skip_frames(time = 2000)
   
9. sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
   
10. sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
    
11. clock = time.clock()
    1 }) t$ @! k5 y+ g( H  t
12. uart = UART(3, 115200)#串口波特率
    
13. 
    ( o+ K& e" C- ^) ~9 h# k
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)
    
17. c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
    
18. 
    6 p( Q* t8 |. A
19. def degrees(radians):
    " Y1 J' \$ i( m) f
20.     return (180 * radians) / math.pi
    
21. 
    : g. h! n* e7 }/ g* K" V
22. while(True):
    
23.     clock.tick()
    
24.     img = sensor.snapshot()
    + o, p' f3 [+ |
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))
    # w2 ?/ W2 F/ }$ m$ M
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()))
    1 |/ A+ C( E8 I% {. d
30.         # Translation units are unknown. Rotation units are in degrees.
    $ k* ]1 t/ g$ k
31.        # print("Tx %f, Ty %f, Tz %f" % print_args)
    / Z7 t- C" L: T2 h" v4 [
32.         uart.write("A%.2f,B%.2f,C%.2f," % print_args+'\r\n')#设置特定格式，以便于stm32分割取得数据
    
33.         #pyb.delay(500)
    5 n# t' L* A  ^) x' I' P" F
34.    # print(clock.fps())</font>

复制代码

STM32主控板（型号为F407）
0 N' K1 ~' W) \( m& a3 D
* C; `( G, @: ]* T9 z) @' l
- I, D) G# k. l0 ]2 M+ D6 I2.1 时钟与中断配置


. H: S) Y9 f7 b) w8 \  P0 i$ e附上stm32时钟示意图：
, s' w) K. C5 R0 ?* o% B. N6 W
定时器示意图：

8 u! x% L5 q' w" x定时器分配：
1 D" I4 p9 B$ I' o2 d
所有时钟初始化的函数：（每个函数的详细内容在后面）

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
   5 u! q. p8 T4 x* {0 `  P' |5 l
4.   TIM2_Int_Init(400-1,20-1);//定时中断，21KHZ
     n- S' a! m0 p! G
5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数，20HZ，50ms中断一次
   
6.   uart_init(115200);  //初始化串口1波特率为115200
   
7.   Encoder_Init_TIM4();//编码器接口初始化</font>

复制代码

2.2 串口收发与数据处理
. t* T% E8 Q3 }5 W9 L, W( j$ \; ]
( f3 |/ d  B6 z- d
& Y( G! d7 e4 K- u( `2 l# m串口中断：USART1，USART2
5 V) u4 m0 f: L0 Z: y  z% U串口初始化函数（以USART1为例）：

1. <font face="微软雅黑" size="3">
   9 k7 S, K" L4 s. s+ c8 d5 T1 Z
2. void uart_init(u32 bound){
     x4 z8 [3 @# G! I0 V, j' L8 e& ^& k& U
3.    //GPIO端口设置
   
4.   GPIO_InitTypeDef GPIO_InitStructure;
   
5.   USART_InitTypeDef USART_InitStructure;
   4 R6 |# v3 z( z3 f- v9 w& i- ^& p
6.   NVIC_InitTypeDef NVIC_InitStructure;
   
7.   
   7 E/ ?; o; D3 k+ c/ Z7 O
8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
   
9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
   + j% s9 G$ q! y
10. 
    
11.   //串口1对应引脚复用映射
    4 P2 j  U% L# d3 }( r
12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
    
13.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
    
14.   
    # m. Z7 u$ B: R. W5 H( j
15.   //USART1端口配置
    + d+ z7 e- S1 \% X: m5 O% m0 q$ @5 Q" g
16.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
    5 w7 C1 E1 E" @1 Z
17.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    
18.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度50MHz
    
19.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
    ( e( y( n( u6 ^
20.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
    
21.   GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9，PA10
      T" d$ A( d- t3 l/ g- P5 q
22. 
    ( j5 Q8 u: C$ m+ C! |/ Z
23.    //USART1 初始化设置
    
24.   USART_InitStructure.USART_BaudRate = bound;//波特率设置
    8 j6 {" I$ c) G# Q7 @
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;//无奇偶校验位
    
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.   
    % A, a5 h0 `$ T" v$ |
32.   USART_Cmd(USART1, ENABLE);  //使能串口1
    7 V  H, S  [+ F* Y# p- ?5 ~9 _
33.   
    
34.   USART_ClearFlag(USART1, USART_FLAG_TC);
    
35.   
    
36. #if EN_USART1_RX  
    
37.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
    
38. 
    * ?4 c" \7 L7 v/ t4 j% O
39.   //Usart1 NVIC 配置
    ! _, Y( n( a, u- V3 s2 i6 q
40.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
    6 P# [% k5 c6 b0 K) t, N) [- I
41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
    9 C+ r  T4 i# A9 C
42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
    ' v/ ^+ m: a7 H  N! l, I
43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
    
44.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、
    
45. 
    - w9 l# z7 i; `" ?2 z9 H" J6 O
46. #endif
    , Z- i- ]6 L6 b& Z- `  N& F) Y
47.   
    7 s' ^; W( V! \* v/ u
48. }</font>

复制代码

串口中断处理函数：

1. <font face="微软雅黑" size="3">
   
2. void USART1_IRQHandler(void)                  //串口1中断服务程序
   
3. {
   * s% @' z) C9 G0 T7 q: v
4.   u8 Res;
   
5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
   
6.   OSIntEnter();   
   + e$ E  c7 s, T" h6 Y8 f
7. #endif
   " y6 }3 x( E% w! P# C- G* W, ]7 ?
8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
   
9.   {
   0 [) m- z! O; z5 {1 _# q
10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
    / A% c( d( h% y) G/ X
11.    
    
12.     if((USART_RX_STA&0x8000)==0)//接收未完成
    ! Y6 {3 z" g: L' p% y( m' J7 s
13.     {
    & S* f' Q/ g% I4 G" O
14.       if(USART_RX_STA&0x4000)//接收到了0x0d
    " f3 [; Z, K3 y3 m) _5 B( |
15.       {
    
16.         if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
    
17.         else USART_RX_STA|=0x8000;  //接收完成了
    
18.       }
    0 {9 c1 R, p& R* x
19.       else //还没收到0X0D
    
20.       {  
    
21.         if(Res==0x0d)USART_RX_STA|=0x4000;
    5 k, N; U. K: P9 r$ W; d9 k
22.         else
    3 x& F' }' n# M) p
23.         {
    ) V; E$ n- S4 y2 B1 n( N
24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
    0 j& C' f/ s# t/ u5 L
25.           USART_RX_STA++;
    
26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
    
27.         }     
    " }- w- _, G+ |9 p5 O
28.       }
    
29.     }        
    
30.   }
    
31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
    5 O7 k: O, s2 C, e* a8 `# S& c- N
32.   OSIntExit();                        
    ; R5 @( n. Z* R  ]
33. #endif
    ! D$ H  G/ g0 C7 a1 V6 g0 W
34. }
    / d2 V# m0 _2 b  P5 @
35. #endif  </font>

复制代码

字符串接收与处理（从openMV接收到的数据）：
+ u: A( r' N, w- n, n

1. <font face="微软雅黑" size="3">/*涉及到的全局变量
   
2. float data[3];//x,y,z方向的距离，浮点数形式
   ' g+ b7 x# N, p4 i0 O3 Z' x7 W. h
3. unsigned char data_string[3][7];//x,y,z方向的距离，字符串形式
   . m/ p6 [8 G0 ~# I8 Q
4. */
   
5. if(USART_RX_STA&0x8000)
   
6.     {  
   2 ~0 `$ f, [- P8 j9 T) e
7.       //清空字符串
   4 j+ Q( I& F3 c$ x+ X! Y
8.       for(i=0;i<2;i++)
   
9.       {
   , ]* `- v/ }6 ^2 X) I
10.         for(j=0;j<6;j++)
    5 g) E8 E6 Y9 `% i! ?4 `
11.         {
    
12.           data_string[i][j]=' ';
    3 W5 a" k5 R5 d
13.         }
    
14.       }
    * X, }0 F& o# ^# X2 {7 |
15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
    ! a! m; G6 d3 N- X0 P- Z0 y
16.       for(t=0,j=0;t<len;t++)
    
17.       {
    : y; @" A8 k7 V9 [8 k9 a  t
18.         j=0;
    
19.         if(USART_RX_BUF[t]=='A')
    
20.         {
    
21.           t++;//去掉首字母
    4 q8 W% O+ D3 |! P3 u
22.           while(USART_RX_BUF[t]!=',')
    
23.           {
    
24.             data_string[0][j]=USART_RX_BUF[t];
      V6 m2 Q' m; c3 l- s8 p
25.             t++;
    : R0 N0 n6 h# o+ y; R% q
26.             j++;
    
27.           }
    
28.         }
    
29.         if(USART_RX_BUF[t]=='B')
    
30.         {
    
31.           t++;//去掉首字母
    + r; i  S8 t+ g
32.           while(USART_RX_BUF[t]!=',')
    
33.           {
    
34.             data_string[1][j]=USART_RX_BUF[t];
      n4 X, P* ?8 ?8 e9 J
35.             t++;
    
36.             j++;
    
37.           }
    ; n! C! R* k( [: m5 G9 W1 c
38.         }
    
39.         if(USART_RX_BUF[t]=='C')
    
40.         {
    
41.           t++;//去掉首字母
    3 v/ H) y$ |1 x3 `# a6 X
42.           while(USART_RX_BUF[t]!=',')
    3 o0 c+ U* [) h, R
43.           {
    ( Q1 p, O1 i2 w' K
44.             data_string[2][j]=USART_RX_BUF[t];
    
45.             t++;
    
46.             j++;
    + R, d7 b& l9 n$ j
47.           }
    - `0 ?' @$ O6 c- w+ ?
48.         }
    
49.       }
    ( I1 t- G2 |& x8 s6 l; Z
50.       //把字符串转化为浮点数
    " \* @9 o1 B$ O; D3 v; T6 a
51.       data[0]=myatof(data_string[0])/100.0;//x
    & V& [. p" ^- D0 h1 o
52.       data[1]=myatof(data_string[1])/100.0;//y
    
53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z，输入是负数，转换为正
    1 h  n6 ?3 f( \2 B# [, q- _$ H
54.         //USART2发送数据
    
55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
    ( y. ]" [+ g# a7 P5 P
56.         
    " }! X# U! f) l  W
57.       //LCD更新显示
    ' W3 W& D, \3 S( w" W
58.       //显示xyz
    - M7 K. I+ J9 D; m0 |; u& c
59. //        CLEAR(10);
    
60. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);
    9 p7 R% H- r# o% S( e" X  N7 N
61. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,data_string[0]);
    0 w/ x( i6 S3 w7 R, ~  _' W
62. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,clear);
    + a5 b+ o1 d. J1 I+ n, f
63. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
    5 I3 f" E+ {) T1 g$ n1 O
64. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
    
65. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);
    
66.         
    
67.         USART_RX_STA=0;//清除标志位
    5 W$ B+ z8 |# M  h: l
68.     }
    
69. }
    
70. </font>

复制代码

字符串转化为两位小数浮点数（用于后续PID控制）:
6 T7 E8 R. m( k! W

1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数，返回的是乘100后的int值，因float返回有错误
   " V  K2 b, o$ U& @" P5 Q: o* ~' @
2. {
   
3.   int flag = 1;//表示正数
   
4.   int res =0;
   * i4 i1 {# A! x- P: S3 C
5.   u8 i=1; //小数点后两位
   
6.   while(*str != '\0')
   1 h" V; K9 o: d- n
7.     {
   
8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
   
9.       {
   
10.         str++;
    
11.         continue;
      G- m; ~, `% Q! o" a
12.       }
    & U, d- v( q; o; d, j1 v
13.       if(*(str-1) == '-')
    
14.       {
    
15.         flag=-1;//表示是一个负数
    
16.       }
    
17. 
    7 Q' {7 y6 q% o) O0 m0 _
18.     while(*str >= '0' && *str <= '9')
    
19.     {
    * X- `  I3 {' F  j! U& f. S! K
20.       res = res *10 + (*str - '0');
    ( l- ^+ F5 ]8 w& y1 t
21.       str++;
    / L1 D4 A6 v1 s& F9 m+ c2 f
22.     }
    - E& j4 o: c! c+ v) q) `
23.     if(*str == '.')
    
24.     {
    2 o6 c( F0 U, u! u: r7 P
25.       str++;
    ! ^9 a' A! ^7 a1 ], H, e
26.       res = res *10 + (*str - '0');
    . V" f) e9 ?; e: o! H' o
27.       str++;
    / b- k2 g4 }& W% k4 I, Q
28.       res = res *10 + (*str - '0');//保留两位，故加两次
    
29.       return res*flag;
    
30.     }
    
31.     }
    
32. }
    ( D" w. M9 P6 g" a
33. 
    
34. 
    ( ~" o6 x9 n. V4 D+ j  d
35. 2.3 LCD显示模块
    0 R5 K  t7 \, u! y
36. 
    % Y& ?5 o$ C) J9 E8 M
37. LCD模块用于调试时观察数据，调试完成可以删去，因为显示屏很耗时，使处理速度变慢
    0 n7 g8 o1 V3 G$ J! t  d
38. 驱动函数总览：
    ( w% {  e; y" N! {) u
39. 
    & s% ~: ?7 I3 W4 _# J
40. 
    8 H* {6 \  R  C9 B5 [/ J
41. 
    + q: S- _' \+ O% u. A9 X2 r
42. void LCD_GPIO_Init(void);
    2 o6 v: ]+ B5 F, L# I
43. void Lcd_WriteIndex(u8 Index);
    
44. void Lcd_WriteData(u8 Data);
    " v  T2 D4 ?# u
45. void Lcd_WriteReg(u8 Index,u8 Data);
    
46. u16 Lcd_ReadReg(u8 LCD_Reg);
    
47. void Lcd_Reset(void);
    
48. void Lcd_Init(void);
    
49. void Lcd_Clear(u16 Color);
    
50. void Lcd_SetXY(u16 x,u16 y);
    
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);
    ' y) j$ D' X2 [0 j& r  v5 q
54. void LCD_WriteData_16Bit(u16 Data);</font>

复制代码

0 T2 m* ~, v9 ~' H2 Y8 T" u  j
TFT屏幕初始化：
8 K; C! Q% [* q) v
( ?$ Q+ ?% y4 N) c( T

1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
   $ \2 P( J% I% A2 f! Z. [8 D
2. {
   
3.   Lcd_Clear(WHITE);
   
4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
     B+ O" b# h$ _( S$ I/ o5 ?9 d
5.   delay_ms(1000);
   3 Y+ g6 H  ?3 ?+ R' F
6.   Lcd_Clear(WHITE);
   & @0 D8 }. c& S2 j9 N# r& k
7.   Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
   ; }/ ?; L0 B$ m* U0 ]: i& a
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:");
    . s" ?* ]" ^, M9 f/ B% @
11. }</font>

复制代码

字符串显示函数;

1. <font face="微软雅黑" size="3">
   
2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
   7 l' O6 H* e8 r4 o6 {: }! _; V
3. {
   
4.   unsigned char i,j;
   
5.   unsigned short k,x0;
   2 {. q6 P) E2 h+ T, g9 u
6.   x0=x;
   3 h4 T$ j3 v7 x! ]. e$ w
7. 
   ' [; V9 r- T6 Z. `6 ?5 J0 {% \
8.   while(*s)
     @* l: E- y( s6 H7 o% G; ?4 t
9.   {  
   
10.     if((*s) < 128)
    8 l. a+ _% d, H, Z( @- h9 }4 ~2 U
11.     {
    
12.       k=*s;
    
13.       if (k==13)
    
14.       {
    % H. \! P6 ?; U
15.         x=x0;
    ) [9 _' U1 y. y* T0 }) ]
16.         y+=16;
    
17.       }
    
18.       else
    & u  d. Q6 i: V' |" p2 u, M2 R7 k
19.       {
    
20.         if (k>32) k-=32; else k=0;
    
21.   
    " c. F* k/ @" G% w6 w
22.           for(i=0;i<16;i++)
    . T9 A* S) F' [# u: R- @3 E, I: ~' r
23.         for(j=0;j<8;j++)
    ; ?+ `* a+ ~" l) u  ?
24.           {
    
25.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
26.             else
    8 \; M+ f+ Q6 m3 I* M, j
27.             {
    . ^! E7 w0 \2 F$ q" A! o
28.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
29.             }
    " N  ~6 e1 ~# s9 ^3 N5 @/ K
30.           }
    . ?$ ?- Z* k. F# r$ q" s
31.         x+=8;
    3 h1 b- z# Z/ [# M$ z5 y5 F0 F
32.       }
    % _% b, t4 p: u1 z7 [  n) x6 B: [- _
33.       s++;
    
34.     }
    / \& k2 X# d- Z! V) v7 u% P
35.       
    
36.     else
    5 c+ P3 Z& D+ X/ M3 x; p
37.     {
    & o! w0 S/ ~: Z2 Q! P' u" ]
38.    
    ; R; E/ M* R  p; S
39. 
    
40.       for (k=0;k<hz16_num;k++)
    ; P5 W$ q+ O% ]$ v5 |/ |
41.       {
    ' y' m- R9 Z3 `5 j
42.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
    
43.         {
    
44.             for(i=0;i<16;i++)
    
45.             {
    , ~& D& S( Z/ b. k7 p  O7 d
46.             for(j=0;j<8;j++)
    & D5 g' a  M) S. d* z; K
47.               {
    
48.                   if(hz16[k].Msk[i*2]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
49.                 else {
    : _* K6 H9 J+ G  ~6 v
50.                   if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
51.                 }
    * N: }* w. C! \9 k& c
52.               }
    
53.             for(j=0;j<8;j++)
    $ D7 B8 \( J4 h$ _% [
54.               {
    ; Z1 z5 A, j2 |: {3 ~& z
55.                   if(hz16[k].Msk[i*2+1]&(0x80>>j))  Gui_DrawPoint(x+j+8,y+i,fc);
    5 c8 }6 _, t4 c# l. L
56.                 else
    * J  O: U# I: G& D. k$ k$ F) y
57.                 {
    
58.                   if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);
    * L& ]; P, i0 i7 f) ~
59.                 }
    
60.               }
    % w. D7 s- v  t5 u) V
61.             }
    
62.         }
    # l2 R, a& t+ z3 ^
63.         }
    + o9 E4 n1 I8 U" O7 f! b( Q" e
64.       s+=2;x+=16;
    
65.     }
    # z; }3 P' S! b+ _5 h
66.    
    
67.   }
    , O0 R# b* v/ Z1 @5 ]) t- R
68. }
    
69. </font>

复制代码

2.4 电机、舵机与编码器

' C8 a9 I: U/ G# r; o
定时中断：TIM2，用于修改电机和舵机的PWM占空比
初始化函数：
8 T+ d  Q" z$ S  ?
9 B7 G/ Q2 f$ t

1. <font face="微软雅黑" size="3">
   
2. //通用定时器2中断初始化
   
3. //arr：自动重装值。
   * b* ~4 m: M9 k5 p, V* S6 l
4. //psc：时钟预分频数
   4 U* M! V! M) Y" |9 ?- |/ b
5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
   
6. //Ft=定时器工作频率,单位:Mhz
   
7. //这里使用的是定时器2!
   
8. void TIM2_Int_Init(u16 arr,u16 psc)
   5 w3 s& p/ p0 s7 ]; P6 w) a4 S9 K+ h
9. {
   
10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    4 u% Z, ~% u+ n" ?$ o
11.   NVIC_InitTypeDef NVIC_InitStructure;
    " I9 J, \5 ~0 z6 P: E8 G$ S) i
12.   
    % ?  _. p% `( R7 n
13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
    0 v" e; G3 ?  l+ M. g# T7 U
14.   
    
15.   TIM_TimeBaseInitStructure.TIM_Period = arr;   //自动重装载值
    
16.   TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    5 k0 X, w: d- o  f. d+ L
17.   TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
18.   TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
19.   
    ' V; W5 C( ~& n, C
20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
    
21.   
    
22.   TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断
    
23.   TIM_Cmd(TIM2,ENABLE); //使能定时器2
    
24.   
    3 J  T2 G8 |  G0 |2 L" `
25.   NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器2中断
    * d: h( G1 J. y3 f4 w0 h# k
26.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级1
    6 G' l' @3 t- \; `7 I3 s2 N- B# q
27.   NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
    
28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    
29.   NVIC_Init(&NVIC_InitStructure);
    
30. 
    6 [4 y. ?' W% j6 {1 ?7 N( M! C" \$ B
31. }</font>

复制代码

TIM2中断处理函数：
# T+ d+ ~' u' v% H: T0 z

1. <font face="微软雅黑" size="3">
   
2. void TIM2_IRQHandler(void)
   
3. {  
   
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   
6.     if(motor_flag==1)//反转
   
7.     {
   , c0 @" _+ P3 T: S2 N1 U
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);
    
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    
15.     }
    ) i0 }+ R& K* a9 x4 O
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，引脚：PA6
    ( ?, \7 Q3 s6 U, c5 a  n0 ~
17.   }
    ! J2 ]6 a6 L* M2 H' L3 C
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    + [) g. X: D) L* S" n
20. </font>

复制代码

PWM输出：TIM3（舵机），TIM8（电机）
初始化函数（以TIM8为例）：

. _% m  S3 ^- O7 O9 {3 Y

1. <font face="微软雅黑" size="3">
   , [& Q0 g! O; D& m
2. void TIM8_PWM_Init(u32 arr,u32 psc)
   
3. {               
   
4.   //此部分需手动修改IO口设置
   
5.   
   2 b& D9 U; n# {) Q& h2 \
6.   GPIO_InitTypeDef GPIO_InitStructure;
   
7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   ! v# c; u6 S! I6 Y/ ^
8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
   
9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
   
10.   
    
11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
    * Q2 G' D  s  B$ v9 {0 r
12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
    
13.   
    
14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
    
15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    ; f! q7 A4 T; k0 @
16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
    % T6 @4 \  d$ T( k
17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    / V( D* ]( W/ i4 _! ^
18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
    ! f+ B$ @9 ~7 B4 K4 n; B
19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7
    
20. 
    
21.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    
22.   GPIO_Init(GPIOB,&GPIO_InitStructure);
    
23.   
    
24.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
    ( _0 v/ L; C6 n- z6 ~% W
25.   GPIO_Init(GPIOC,&GPIO_InitStructure);
    6 f6 M6 A. _8 Z& N3 ]
26.   
    6 d# a9 d5 f3 {1 h4 j" E
27.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
    
28.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1
    
29.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
    / V8 Z9 _2 `, b
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
    5 X& E; }: Y7 H1 Z; Y
33. 
    4 v! p* E' Q0 f: F0 ]& R
34.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
    ! d- O! O: `9 L6 [+ n; e$ ^
35.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    0 @2 p& ^/ w1 T8 H) @( U
36.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
    
37.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
38.   
    
39.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
    + O' j9 N0 e- [1 p6 d
40.   
    
41.   //初始化PWM模式   
    , Y" T. d* m& O
42.   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    ' y7 m4 {# f& B9 s
43.   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    - a5 z9 _! y, [
44.   TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    $ f9 [3 }8 v4 X
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;
    
49.   TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
      Z7 }5 A8 a) b/ ~
50. 
    
51.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    
52.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    0 J  Y( d7 ?, ~( u8 s
53.   TIM_OC3Init(TIM8, &TIM_OCInitStructure);
    + T# d- \. A0 A, n; X( Z8 ^% {
54. 
    * q4 d0 o+ u- \& i: C6 e, P
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;
    % q# i" w( g: G/ u$ X- p* m* |/ ^
60.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
    3 R  x. v% N2 p, U; `
61.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
    
62.   TIM_BDTRInitStructure.TIM_DeadTime = 0;
    
63.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
    3 k  V" z" X  R% \
64.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;
    
65.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
    
66.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);
    
67. 
    
68.   TIM_Cmd(TIM8,ENABLE);
    , T0 G/ q, X& ]  D) r7 g
69.   TIM_CCPreloadControl(TIM8,ENABLE);
    9 N0 o: W' Z! w$ }% @
70.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
    * U( Q3 e" n1 g2 _* X% i( |% ?/ Y) i
71.                      
    
72. }</font>

复制代码

编码器初始化函数：

1. <font face="微软雅黑" size="3">
   + J& @+ w+ ]5 p9 r; K& s1 j
2. void Encoder_Init_TIM4(void)
   
3. {
   8 q. G& h0 q. {: |% ]$ H! u
4.     GPIO_InitTypeDef         GPIO_InitStructure;
   
5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   ' {2 g0 r' z/ W" ~" Y1 X$ l4 j
6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
   
7.     NVIC_InitTypeDef NVIC_InitStructure;
   
8. 
   
9.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
   
10.     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟
    - l  L# e3 D" Y: U
11.   
    & @+ B! T8 u% t0 x3 {' N
12.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用
    " U5 L" ~: z- U5 m& R
13.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用
    
14. 
    
15.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
    9 J0 P' A( ?* ?! d( ~: ^
16.     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
    $ t* d# p# t: j# v
17.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    
18.     GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    4 U5 O5 V. C; N  I( D
19.     //GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL ;
    ' J- g) M9 F3 G& O3 @! l
20.     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
    
21.     GPIO_Init(GPIOB,&GPIO_InitStructure);
    ' p' r0 u* r4 S1 J4 E# D0 o# ?
22.    
    
23.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    
24.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
    
25.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    
26.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断，防止计数溢出而没有相应函数，造成卡死
    
27.     NVIC_Init(&NVIC_InitStructure);
      X: V0 d) Z9 r; ?' u8 ]
28. 
    
29. 
    
30. 
    $ E  |7 I5 O- `: F
31.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
      i" L6 H0 _3 B2 V  @5 i+ I( M
32.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
    4 A+ }. o4 l2 Q5 L& e
33.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
    0 Y, G; }0 F, j, @/ T/ z
34.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    
35.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    & l  T( K) v7 H4 u9 w+ v! `# [( `
36. 
    
37.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
    
38.     TIM_ICStructInit(&TIM_ICInitStructure);
    
39.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
    
40.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
    
41.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
    2 P7 E' k: c0 z  S3 b6 M8 _: L% g3 @  F
42.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
    1 V4 W: l5 O4 D3 }8 D6 \( i' i
43.     TIM4->CNT = 0;
    % L& u* Y  `- [' ^! F3 _( }
44.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
    
45. }
    
46. </font>

复制代码

编码器返回速度值：
# K6 L; Z, w0 J) X4 x  i# F$ W

1. <font face="微软雅黑" size="3">/**************************************************************************
   ' P6 U2 `& S+ G: D
2. 函数功能：单位时间读取编码器计数
   $ i/ ~/ d/ }7 K" x9 d
3. 入口参数：定时器
   
4. 返回  值：速度值
   ) _( S2 Q- f- J* Y0 m" I
5. **************************************************************************/
   9 F* \2 ]* U% T" [. B$ Q! u' U
6. float Read_Encoder_Speed(uint8_t TIMX)
   , s3 F# |8 Q& o8 E( g6 a
7. {
   ; L* c; c4 T: ?  X+ m5 [
8.     int32_t Encoder_TIM;
   + p% }! r$ s3 _
9.     float res = 0;
   
10.     switch (TIMX)
    
11.     {
    " ?8 Z5 T2 U  |' `: f2 a$ _+ [
12.     case 5:
    
13.         Encoder_TIM = TIM_GetCounter(TIM5);
    
14.         TIM5->CNT = ENCODER_BASE_COUNT;
    * i! \/ M" o" X
15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    
16.         break;
    
17.     case 4:
    & H( v& I+ X3 M
18.         Encoder_TIM = TIM_GetCounter(TIM4);
    
19.         TIM4->CNT = ENCODER_BASE_COUNT;
    ) e3 J% P" \# D% s$ j8 X0 |. {/ Y9 Z
20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    ) r* X) i3 V7 F7 W
21.         break;
    , F, N/ y/ e7 f. F
22.     default:
    % q6 k" R! T0 P; A
23.         Encoder_TIM = 0;
    " }5 n* \: R+ I+ S, s2 O3 d
24.         res = 0;
    , `& P& u+ q. S$ k" B0 h
25.     }
    2 J4 T9 X3 S% V5 I
26.     if(res>2048.0f)
    $ a: t! z& I* ~. F0 i% k1 u
27.         res-=4096.0f;
    6 v8 H, m6 d0 Q8 ~
28.     return res*360.0f/4096.0f;
    
29. }
    - w! X! {. `9 v6 \
30. </font>

复制代码

定时从编码器取数，注意，时间不一样，取回的数值也不一样，取决于实际速度以及编码器线数。这里50ms取一次：
9 U9 q' `. w  t) W

1. <font face="微软雅黑" size="3">
   * {  Y5 \( L, F. q) K8 V
2. void TIM7_IRQHandler(void)//频率20HZ，用于编码器计数
   7 T1 r2 V& ~7 D8 f: @
3. {  
   - o, O9 i. w4 j$ d6 m
4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   
6.     speed=Read_Encoder_Speed(4);
   
7.   }
   & f' D* g+ S. {, v0 I5 W8 Q# y# t  l. o
8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
   
9. }</font>

复制代码

2.5 PID控制
: q. R4 X: I8 w8 Y
* N$ G& y) H# G5 h5 z
PID库函数：
* j4 |. }  i% Z

1. <font face="微软雅黑" size="3">
   
2. #define N 2        //需要对多少变量进行pid调节
   6 A* A8 i8 O. I: R1 x9 b: N
3. 
   2 w2 }' e1 F, w# D
4. const float KP[N]={1.3,1.0};//这里只用了比例调节
   , O/ P1 r/ m: l! i, K$ P
5. const float KI[N]={0,0};
   ; l! f) K& h8 {. [+ x* w4 X6 A. s0 [
6. const float KD[N]={0,0};
   
7. 
   
8. 
   
9. struct _pid{
   * [' m' Z8 a" O9 ~% D8 O
10.   float SetVol;        //定义设定值
    
11.   float ActVol;        //定义实际值
    
12.   float Err;          //定义误差
    
13.   float  Err_Next;      //定义上一个误差
    / |, @  o3 V( u/ V8 n. C
14.   float  Err_Last;      //定义上上一个误差
    
15.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
    
16.   float integral;      //定义积分值
    6 i, [( b/ w7 ]  i0 L
17.   float actuator;      //定义控制器执行变量
    
18. }pid[N];
    
19. 
    
20. void PID_Init(void)
    
21. {
    3 k4 N) ]) C/ l7 B( Z6 q7 r& u
22.   for(int i=0;i<N;i++)
    
23.   {
    5 a$ f. A* m5 c# `
24.     pid[i].SetVol=0.0;
    * n& O$ P% b3 @( M
25.     pid[i].ActVol=0.0;
    4 Z' B+ d1 X- u! l7 R- r5 e
26.     pid[i].Err=0.0;
    
27.     pid[i].Err_Next=0.0;
    - k- v+ w: R. T$ w5 w
28.     pid[i].Err_Last=0.0;
    
29.     pid[i].integral=0.0;
      R2 ]8 L6 [$ y3 L) G; ^4 u
30.     pid[i].actuator=0.0;
    - b( T" q! k* }  D6 l6 o0 d  o
31.     pid[i].Kp=KP[i];
    & t# R( O( V- u" t. d
32.     pid[i].Ki=KI[i];
    
33.     pid[i].Kd=KD[i];
    
34.   }
    " H3 x. w9 D2 k
35. }
    * p% m: w9 [! x9 w$ |' g" Z
36. 
    1 F  _4 O" J: y3 n* a
37. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
    8 e( z' [- }& H! A& N' `6 h
38. {
    
39.   pid[i].SetVol=set_val;
    
40.   pid[i].ActVol=get_val;
    
41.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
    % U+ C$ k1 L$ o/ U
42.   float IncVol;    //定义增量
    % r0 f- C6 p9 Z$ p( Y+ h
43.   pid[i].integral+=pid[i].Err;
    
44. //  IncVol=pid[i].Kp*(pid[i].Err-pid[i].Err_Next)+pid[i].Ki*pid[i].Err+pid[i].Kd*(pid[i].Err-2*pid[i].Err_Next+pid[i].Err_Last);
    
45.   pid[i].actuator=pid[i].Kp* pid[i].Err+pid[i].Ki*pid[i].integral+pid[i].Kd*(pid[i].Err-pid[i].Err_Next);
    
46. //  pid[i].actuator=adc_val+IncVol;
    ! Y3 N/ b0 k* l4 _+ e( ?$ B
47.   pid[i].ActVol=pid[i].actuator;
    
48.   pid[i].Err_Last=pid[i].Err_Next;
    
49.   pid[i].Err_Next=pid[i].Err;
    9 O" P. H3 C3 _# t
50.   
    
51.   return pid[i].actuator;
    
52. }</font>

复制代码

主函数中的PID调节：
! Y# [1 I" h% ^

1. <font face="微软雅黑" size="3">
   
2.     z_get=data[2];
   ' Q/ h' G3 G+ t! b( s% T/ F
3.     x_get=data[0];
   % c- R$ ?' v* j" f
4.     if(z_get-z_set>0.5||z_get-z_set<-0.5)//电机PID
   
5.     {
   3 j7 a  C% p4 ?. v% O
6.       LED1=0;  //调节时灯亮
   : g! `2 S% m" V' u
7.       PID_val_motor=PID_realize(z_set,z_get,0);
   9 W3 M9 F& w' I
8.       PID_val_motor=PID_val_motor/10.0;
   ; A) R, ?- r' F+ m7 S
9.       if(PID_val_motor<=0)
   2 d9 ^1 b0 [' s4 a5 b1 J- g3 j* P3 P7 A
10.         motor_flag=0;//motor_flag控制电机正反转，PID_val_motor用于改变占空比，范围0~1
    
11.       if(PID_val_motor>0)
    ( g1 E/ i3 ]* R
12.         motor_flag=1;
    
13.       PID_val_motor=abs_float(PID_val_motor);
    ( B( q) `1 A7 L9 f7 z6 d; o; G
14.       if(PID_val_motor>2)PID_val_motor=0;//标志太远，让车停止
    
15.       if(PID_val_motor>1&&PID_val_motor<=2)PID_val_motor=1;
    8 m" h" G/ s2 }, E% _: Z- L2 Q8 N
16.       if(PID_val_motor<0.2)PID_val_motor=0;
    
17.     }
    
18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
    
19.     {
    
20.       LED1=0;  
    & C( }) e0 `) M0 e* q8 j
21.       PID_val_servo=PID_realize(x_set,x_get,1);
    - |, o% d* ~6 h' w, R
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;
    4 v5 ~3 E1 s+ K6 G* \
25.     }
    
26.     LED1=1;</font>

复制代码

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

1. <font face="微软雅黑" size="3">
   
2. void TIM2_IRQHandler(void)
   / U; R0 v* v- T! u( }& p
3. {  
   - ?+ L' k3 L2 \: w" O
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   5 b9 u% B8 k8 T
5.   {
   1 F3 O. k2 r  g- E
6.     if(motor_flag==1)//反转
   
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   
9.       TIM_SetCompare2(TIM8,0);
   ) @+ R+ P- v5 @" q( R* |
10.     }
    
11.     if(motor_flag==0)//正转
    
12.     {
    
13.       TIM_SetCompare1(TIM8,0);
    6 v6 F5 }$ o$ ]& _
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
    
17.   }
    
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    
20. </font>

复制代码

* m1 O3 `, L2 t3 ^
1 n" r0 B& E0 a/ g! l

优秀
& J( h! o' ?* g3 F6 H" ~3 H