概述
; B, I' P5 N4 }/ Z; e! ~& H: q1 |7 |4 `小车外形：
# v2 C9 v! p& o, K8 l" e4 M
2 i' y% E$ x+ ~+ z: B' ^

2 V: P* I  u9 T3 S6 D4 Y功能简介
利用摄像头识别前车尾部的AprilTag，得到前车位置，传回stm32主控板处理，使两车在行驶时保持恒定距离，实现自动跟车。

5 R8 l7 c+ j$ P1 X' p% k- R


5 a1 ?/ b4 P$ f% KopenMV4摄像头
9 w7 A4 z3 j5 v, U. A4 O7 c; Q1.1 Apriltag识别与串口传输

% J3 S# \5 X! y0 k
1 r# J' ^; D' v- F2 HAprilTag是一个视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志（与二维码相似，但是降低了复杂度以满足实时性要求），可以快速地检测标志，并计算相对位置。


* M# U' E- Z3 {) q8 d: R! S8 |Apriltag示例：
9 U: Q; W+ c  r3 W* k/ T5 b
通过识别Apriltag，可以得到x,y,z三个方向的距离以及偏移角度。这里只需要三维的距离即可，通过串口传回stm32.

1. <font face="微软雅黑" size="3">
   6 D1 u- K$ g( N5 c; Q4 X
2. import sensor, image, time, math,pyb
   
3. from pyb import UART
   
4. 
   ' D# W3 E( d/ e  u
5. sensor.reset()
   ' }2 l4 z% u- ^
6. sensor.set_pixformat(sensor.RGB565)
   3 E5 e: R1 [* a5 d
7. sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
   
8. sensor.skip_frames(time = 2000)
   % b' _4 b* o8 X' p# g! s4 t4 u5 b
9. sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
   5 P# \1 A5 N7 h0 B' J1 z; E$ A+ p
10. sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
    + n/ N. x" F; F$ k0 d
11. clock = time.clock()
    0 M. }, y2 ?* g" m3 T
12. uart = UART(3, 115200)#串口波特率
    5 ~3 m. j: ]0 ]
13. 
    ! X- H- l9 h7 h% W3 G
14. f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
    & [0 k( ^2 z( `. G
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)
    6 \" |0 b& W5 z' r) t( S! w" d  ~
17. c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
    
18. 
    2 ?! S! L5 b; P
19. def degrees(radians):
    / y! y$ s  W# z6 H6 g! {
20.     return (180 * radians) / math.pi
    
21. 
    
22. while(True):
    % Q) r2 S1 G: r3 H7 a) n
23.     clock.tick()
    
24.     img = sensor.snapshot()
    , h) |- H/ L, e
25.     for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11
    $ O) Z7 h9 r& d: x) e0 ]" O
26.         img.draw_rectangle(tag.rect(), color = (255, 0, 0))
    5 G3 u7 M3 G& _" E" m
27.         img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
    
28.         print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
    5 l9 p# f* A' H  f: L0 d
29.             #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
    4 y9 g& f4 P, \% K4 |" n4 V
30.         # Translation units are unknown. Rotation units are in degrees.
    
31.        # print("Tx %f, Ty %f, Tz %f" % print_args)
    ! T$ j' d3 m+ J0 a5 e+ D4 ^- ]
32.         uart.write("A%.2f,B%.2f,C%.2f," % print_args+'\r\n')#设置特定格式，以便于stm32分割取得数据
    % d# o4 f/ D8 P/ i/ ]. X
33.         #pyb.delay(500)
    7 ?# A' M6 J3 y" i* y  }
34.    # print(clock.fps())</font>

复制代码

STM32主控板（型号为F407）


2.1 时钟与中断配置
# Q9 y' V- d  p

' j7 r# v. w0 F  u$ a附上stm32时钟示意图：

定时器示意图：
0 ~1 r2 |1 i3 o- F) J
+ v  c' }7 d1 ^! U5 L+ ?定时器分配：

所有时钟初始化的函数：（每个函数的详细内容在后面）
* Q6 w/ t# P) B' c4 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
   
5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数，20HZ，50ms中断一次
   " m: p; L* p" ?  K  h; k* |+ y
6.   uart_init(115200);  //初始化串口1波特率为115200
   
7.   Encoder_Init_TIM4();//编码器接口初始化</font>

复制代码

2.2 串口收发与数据处理
) _8 Y+ A( `* ]! `9 `! ^

串口中断：USART1，USART2
串口初始化函数（以USART1为例）：
; B9 m5 i' f4 c6 m

1. <font face="微软雅黑" size="3">
   
2. void uart_init(u32 bound){
   
3.    //GPIO端口设置
   $ o2 b! A/ Z* b0 v6 A
4.   GPIO_InitTypeDef GPIO_InitStructure;
   ! z( Q- q, @0 N' @7 f
5.   USART_InitTypeDef USART_InitStructure;
   
6.   NVIC_InitTypeDef NVIC_InitStructure;
   & a1 p" [: a; [0 I$ `1 S0 J) D
7.   
   # G# N" ?0 t; v# A
8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
   # D' {% s$ `- P9 i" i7 ~$ W
9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
   : h' ^1 t$ p3 F2 A% E
10. 
    
11.   //串口1对应引脚复用映射
    3 Z9 N' u! {7 H( ?# }& R
12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
    
13.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
    , L7 Q! ]) i, ?7 l  e: a2 D* e
14.   
    
15.   //USART1端口配置
    & O7 J4 m! y/ D2 n, _5 B" ]! ^! D
16.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
    % y$ [% v0 w1 ^9 R
17.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    + K$ H' s- `6 \4 x# C$ g
18.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度50MHz
    
19.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
    , a: S7 Z6 ?8 q3 w1 h! q3 {/ n
20.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
    " c; i$ T4 E8 P1 Q6 N% x/ _
21.   GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9，PA10
    
22. 
    
23.    //USART1 初始化设置
    * ~" F0 T; `. ]5 Z/ z
24.   USART_InitStructure.USART_BaudRate = bound;//波特率设置
    - e: e) |! S. ]3 g& w; Q: U8 Z' U
25.   USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
    $ M& q0 L! o* s2 \, P2 v  [
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.   
    
32.   USART_Cmd(USART1, ENABLE);  //使能串口1
    
33.   
    + X& H( I4 n  {; a6 G( v
34.   USART_ClearFlag(USART1, USART_FLAG_TC);
    - s$ C7 d1 I$ J: W" ?2 a
35.   
    * i% o2 V) K7 l" P8 O$ E  `$ c
36. #if EN_USART1_RX  
    8 e/ m3 e; {5 N: A
37.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
    # B! Q6 D! ?. n
38. 
    
39.   //Usart1 NVIC 配置
    , F: O2 J7 Q. d! m# c& v# y+ a" S
40.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
    + g% \2 G& `" S$ d- W( v
41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
    " D; E4 z0 _0 N3 b6 H- M9 v
42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
    
43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
    
44.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、
    & `3 t3 U  i1 h5 @8 ?
45. 
    0 h! y0 }" A+ P% f
46. #endif
    
47.   
    3 ~7 C2 Q/ T9 ~- x/ Q; T& X, E; Z
48. }</font>

复制代码

串口中断处理函数：
) j& M' J5 }; _! K8 U+ _3 ?  e

1. <font face="微软雅黑" size="3">
   ' E9 p* s# k) X/ {" ^; b9 d" P& K
2. void USART1_IRQHandler(void)                  //串口1中断服务程序
   3 i8 o# R) \  \  A3 g3 h
3. {
   9 d3 U5 L" Q' ?7 Y5 H! \. [
4.   u8 Res;
   
5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
   7 O3 k& w9 s' w6 ]# g8 i
6.   OSIntEnter();   
   1 n: f) o! M& i' S1 X
7. #endif
   
8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
   6 V1 `% e( b# H
9.   {
   ' j0 N( {) I& f! m& w. Z4 p* \
10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
    0 z+ {0 b% Q; a4 Q7 }. Z5 Z1 F
11.    
    
12.     if((USART_RX_STA&0x8000)==0)//接收未完成
    
13.     {
    
14.       if(USART_RX_STA&0x4000)//接收到了0x0d
    
15.       {
    $ E( [1 {6 {9 A( a8 y5 Y$ b6 b2 Y) \
16.         if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
    
17.         else USART_RX_STA|=0x8000;  //接收完成了
    2 G5 W: n& u( I0 ?* w: {# h: L3 J! T- @
18.       }
    
19.       else //还没收到0X0D
    % C2 G" {1 m- J; x: I  J* z
20.       {  
    2 M  t* K5 {( e; z  ?
21.         if(Res==0x0d)USART_RX_STA|=0x4000;
    
22.         else
    
23.         {
    
24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
    3 F% Y' j7 Y% d3 o6 Y8 s
25.           USART_RX_STA++;
    
26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
    
27.         }     
    
28.       }
    
29.     }        
    : J+ b& J+ a' @/ v
30.   }
    
31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
    ; c9 ^3 M( G% u
32.   OSIntExit();                        
    
33. #endif
    
34. }
    
35. #endif  </font>

复制代码

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

1. <font face="微软雅黑" size="3">/*涉及到的全局变量
   . F0 L! o1 f/ r
2. float data[3];//x,y,z方向的距离，浮点数形式
   2 H% n7 |- u$ c0 ^, X& v" F
3. unsigned char data_string[3][7];//x,y,z方向的距离，字符串形式
   . {, w2 T) Y, p$ L
4. */
   
5. if(USART_RX_STA&0x8000)
   
6.     {  
   % W' P4 D5 n" U: O  u; S
7.       //清空字符串
   8 J5 J! h$ q8 }; n' _4 J
8.       for(i=0;i<2;i++)
   
9.       {
   8 d; ~5 l( G; e% m3 A
10.         for(j=0;j<6;j++)
    9 b" s: L/ b# ]9 P" J: P6 s& M
11.         {
    
12.           data_string[i][j]=' ';
    1 e% n* H! Q& n* k$ G
13.         }
    
14.       }
    
15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
    
16.       for(t=0,j=0;t<len;t++)
      p. O( i% m2 m- n
17.       {
    
18.         j=0;
    
19.         if(USART_RX_BUF[t]=='A')
    
20.         {
    
21.           t++;//去掉首字母
      ?9 @. s$ I; T3 ^7 k+ U
22.           while(USART_RX_BUF[t]!=',')
    
23.           {
    
24.             data_string[0][j]=USART_RX_BUF[t];
    
25.             t++;
    " E% n4 ^% B: z
26.             j++;
    
27.           }
    9 Z" U% V! l' A8 S' F$ v
28.         }
    
29.         if(USART_RX_BUF[t]=='B')
    
30.         {
    ! }2 `8 r6 T0 N! l
31.           t++;//去掉首字母
    1 s1 Y& u2 {; J6 v( G2 o' K7 S
32.           while(USART_RX_BUF[t]!=',')
    3 b1 Z) u. ?& S$ Z; F/ n% L
33.           {
    
34.             data_string[1][j]=USART_RX_BUF[t];
    8 |, O- u) h2 {1 r4 T- U8 [
35.             t++;
    
36.             j++;
    
37.           }
    
38.         }
    
39.         if(USART_RX_BUF[t]=='C')
    
40.         {
    2 p/ }0 o- C; u( e+ v+ H" r% d
41.           t++;//去掉首字母
    
42.           while(USART_RX_BUF[t]!=',')
    
43.           {
    
44.             data_string[2][j]=USART_RX_BUF[t];
    : y2 {" j7 W6 \% i5 i5 T, O5 @2 S+ _
45.             t++;
    
46.             j++;
    ) v" e6 `* Q6 y: u
47.           }
    / M" S. s: f1 N& X
48.         }
    
49.       }
    . _: R; k- V- ?' c3 i; b0 i
50.       //把字符串转化为浮点数
    
51.       data[0]=myatof(data_string[0])/100.0;//x
    1 w* U+ Z0 e3 \3 U( j- e  G
52.       data[1]=myatof(data_string[1])/100.0;//y
    
53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z，输入是负数，转换为正
    
54.         //USART2发送数据
    
55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
    
56.         
    
57.       //LCD更新显示
    3 h& C# C0 d; |4 v+ c- w* h
58.       //显示xyz
    4 m/ q& R1 k% l( u0 J/ V8 @
59. //        CLEAR(10);
    ( U' {, ?0 J! o( n2 p5 Z* H, E% d
60. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);
    
61. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,data_string[0]);
    : G, d/ x" {  \* X1 Y
62. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,clear);
    8 P: m# r" y" ^. `$ G4 ?5 B1 b
63. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
    % e3 [2 F' }) R/ H, }  a; s
64. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
    
65. //        Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);
    9 U& F/ Q. p. n6 i0 H: I( a
66.         
    ' `' Z2 u3 t* A
67.         USART_RX_STA=0;//清除标志位
    
68.     }
    ! U3 Q  f: Q  B; S! H( i4 I
69. }
    
70. </font>

复制代码

字符串转化为两位小数浮点数（用于后续PID控制）:
( d- e8 o6 X# ]/ k
( V; {( ]. k: P% M  c( u

1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数，返回的是乘100后的int值，因float返回有错误
   
2. {
   4 W- c* O* c2 _! F% I, n* W
3.   int flag = 1;//表示正数
   
4.   int res =0;
   + U4 @5 V3 R! v4 ^
5.   u8 i=1; //小数点后两位
   ) `2 l# U1 [2 I0 m
6.   while(*str != '\0')
   
7.     {
   
8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
   
9.       {
   
10.         str++;
    
11.         continue;
    
12.       }
    
13.       if(*(str-1) == '-')
    
14.       {
    
15.         flag=-1;//表示是一个负数
    
16.       }
    ! G! w3 N  e5 a+ x' [4 U
17. 
    * x+ V9 T" Z. m5 A" c
18.     while(*str >= '0' && *str <= '9')
    
19.     {
    ; v- A7 }( _7 s- k
20.       res = res *10 + (*str - '0');
    
21.       str++;
    
22.     }
    6 H3 Y& {. U5 ^
23.     if(*str == '.')
    5 r. c6 M- {- ~9 u' L
24.     {
    
25.       str++;
    ( E* u/ K1 g) r! t; w
26.       res = res *10 + (*str - '0');
    4 B2 B( l! E8 K4 w8 U2 {
27.       str++;
    
28.       res = res *10 + (*str - '0');//保留两位，故加两次
    5 T# S9 _& w' Z( ]4 p/ b
29.       return res*flag;
    
30.     }
    
31.     }
    
32. }
    
33. 
    
34. 
    : d& u$ V; a, m/ W+ e
35. 2.3 LCD显示模块
    ! f! n% R2 G/ G: A$ u8 X
36. 
    8 P  F7 v4 n1 G$ O7 Z
37. LCD模块用于调试时观察数据，调试完成可以删去，因为显示屏很耗时，使处理速度变慢
    
38. 驱动函数总览：
    
39. 
    
40. 
    3 I" A5 ]1 I& K; _: Q$ L
41. 
    
42. void LCD_GPIO_Init(void);
    
43. void Lcd_WriteIndex(u8 Index);
    " l2 _  p1 e2 T; P% \
44. void Lcd_WriteData(u8 Data);
    / @7 w0 ~" i( K! R8 Q
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);
    * o+ y2 O# s! O- O
49. void Lcd_Clear(u16 Color);
    % j$ s5 T) f3 O+ g
50. void Lcd_SetXY(u16 x,u16 y);
    
51. void Gui_DrawPoint(u16 x,u16 y,u16 Data);
    2 J0 A$ s3 L7 f- m
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);
    ; X. {/ z, `- o6 A; P
54. void LCD_WriteData_16Bit(u16 Data);</font>

复制代码

" r1 N( X5 q: D
TFT屏幕初始化：

# B3 _. A7 z9 @1 G, g2 o  b( S

1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
   4 ^9 o8 |- \2 t7 c" n
2. {
   
3.   Lcd_Clear(WHITE);
   ) u, ~! M( d- H2 Q
4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
   
5.   delay_ms(1000);
   3 {' I, S9 ]8 l/ ]
6.   Lcd_Clear(WHITE);
   ' z! e; B6 E+ |1 X
7.   Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
   
8.   Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:");
   9 N" d/ _6 T9 X, }: B$ G/ c8 F
9.   Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:");
   2 D5 k# t4 O9 H) Z
10.   Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");
    1 b0 ~4 q0 [& e& d) T  m
11. }</font>

复制代码

字符串显示函数;
# k* L2 [+ V9 z6 m5 N

1. <font face="微软雅黑" size="3">
   
2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
   
3. {
   7 r! [9 M+ n2 C5 r/ O
4.   unsigned char i,j;
   
5.   unsigned short k,x0;
   
6.   x0=x;
   
7. 
   & o4 M3 h+ Q1 U
8.   while(*s)
   $ i6 u. g  W' _) e& M  \6 w
9.   {  
   
10.     if((*s) < 128)
    
11.     {
    : z2 P8 j+ d# u4 c
12.       k=*s;
    : r5 |+ J. h+ @" x0 u9 Y1 M
13.       if (k==13)
    
14.       {
    % u& E/ ?' v& e' V* j4 Y
15.         x=x0;
    $ C5 F5 O9 o# P0 }' O; ^7 r6 L3 l
16.         y+=16;
    
17.       }
    
18.       else
    * d/ |1 g2 x4 o- V5 [
19.       {
    
20.         if (k>32) k-=32; else k=0;
    
21.   
    + }/ R- I5 l. y' Y
22.           for(i=0;i<16;i++)
    
23.         for(j=0;j<8;j++)
    $ s' s- T" D6 t  h9 H! d/ }: T2 `
24.           {
    
25.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
26.             else
    
27.             {
    5 q2 {/ g+ f& q* w* ], A/ F
28.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
29.             }
    
30.           }
    
31.         x+=8;
    ! H* f1 i! b; V  v/ P
32.       }
    ) d$ {) `4 _) f  W! [+ k" f
33.       s++;
    " l% k2 f3 u8 q, W) W4 v6 X& P
34.     }
    
35.       
    
36.     else
    $ P- N5 U. ?6 V. J$ n3 J
37.     {
    2 b& \9 G% l8 K2 ]6 {
38.    
    
39. 
    - p$ B" U4 |# l9 x" I
40.       for (k=0;k<hz16_num;k++)
    
41.       {
    6 X" t/ g( T0 w( S6 t
42.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
    - \; t4 ]) F0 ]  A
43.         {
    
44.             for(i=0;i<16;i++)
    0 g. W* b# E) J* A4 Z4 n; V' ?
45.             {
    
46.             for(j=0;j<8;j++)
    
47.               {
    
48.                   if(hz16[k].Msk[i*2]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    : X+ G- n! x! i( b2 G1 r6 }' ^: D
49.                 else {
    
50.                   if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    ! O0 \& C# I* j$ X6 d% x% N
51.                 }
    4 V# j3 ?! ~7 j0 c" k2 z2 p  X5 r3 ]
52.               }
    . g" x+ \5 e# g% {
53.             for(j=0;j<8;j++)
    " ?1 }$ g$ w+ I! i; \3 @
54.               {
    $ r7 R0 y2 d, E  i+ q' ^
55.                   if(hz16[k].Msk[i*2+1]&(0x80>>j))  Gui_DrawPoint(x+j+8,y+i,fc);
    
56.                 else
    
57.                 {
    
58.                   if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);
    6 U* _' P& u; h% O& W% v0 k
59.                 }
    / ]7 W/ j' f; Q0 A
60.               }
    ) ?1 @) f% E: b6 Y6 }3 W
61.             }
    2 O' k9 u" \# c& v% ?5 l% p
62.         }
    
63.         }
    
64.       s+=2;x+=16;
    
65.     }
    9 e" m9 ^$ S7 ]0 t
66.    
    3 Z+ n" t0 H; D  V- G0 b9 Y
67.   }
    
68. }
    / r$ K! M+ Z& M2 T0 S) X6 D
69. </font>

复制代码

2.4 电机、舵机与编码器

/ e+ S3 r! j$ e6 y& Y8 t
定时中断：TIM2，用于修改电机和舵机的PWM占空比
初始化函数：
3 |0 n/ N8 q' c; a: m- U: Q
* ]2 V& e3 g: Q  F0 U$ b

1. <font face="微软雅黑" size="3">
   # Q0 _& [' e! [
2. //通用定时器2中断初始化
   8 ^! C( q1 f( V  g: n
3. //arr：自动重装值。
   
4. //psc：时钟预分频数
   
5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
   7 _% ^# d' y! N  o% ?
6. //Ft=定时器工作频率,单位:Mhz
   & i- L/ `/ h, H6 o7 w  F
7. //这里使用的是定时器2!
   
8. void TIM2_Int_Init(u16 arr,u16 psc)
   
9. {
   ' Z% Y/ b0 o6 O" p
10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    
11.   NVIC_InitTypeDef NVIC_InitStructure;
    
12.   
      z' ^2 S" [- K2 w
13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
    
14.   
    
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;
    : z0 m7 \/ A( Y1 Y; I  A$ a- F
19.   
    $ f+ R% b) J% A4 T% W
20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
    ) B. M) d8 z) D8 H0 l9 S4 L+ d
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中断
    
26.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级1
    
27.   NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
    $ D4 ~3 q5 I  e3 Y& u
28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    
29.   NVIC_Init(&NVIC_InitStructure);
    5 ^( e/ C2 z& Y- H  \- L7 |
30. 
    
31. }</font>

复制代码

TIM2中断处理函数：
: o- A6 e5 b2 F2 b6 M/ N' ?  V- ]* B

1. <font face="微软雅黑" size="3">
   
2. void TIM2_IRQHandler(void)
   
3. {  
   . e" i$ x! B$ D  U" E# Q
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   5 |- n6 b5 A) ^4 `+ h) @
5.   {
   
6.     if(motor_flag==1)//反转
   
7.     {
   - ^+ Q8 W/ ~$ y; ?
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   
9.       TIM_SetCompare2(TIM8,0);
   - x& b4 G& ]% G) @
10.     }
    
11.     if(motor_flag==0)//正转
    " F& s( I( W8 ]$ S1 T  \
12.     {
    4 }6 O; V6 x. g0 c5 Z5 @
13.       TIM_SetCompare1(TIM8,0);
    ! q% G' j* E: J/ h; T* t# }* N/ Q
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    
15.     }
    5 s# s% v3 H& d( Z( Q+ y6 i' Z
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，引脚：PA6
    
17.   }
    $ c. V' z! x2 w' Q; v2 A6 r
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    
20. </font>

复制代码

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

1. <font face="微软雅黑" size="3">
   
2. void TIM8_PWM_Init(u32 arr,u32 psc)
   
3. {               
   
4.   //此部分需手动修改IO口设置
   
5.   
   6 y/ G4 q) N  _) e
6.   GPIO_InitTypeDef GPIO_InitStructure;
   
7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   
8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
   
9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
   5 z2 Y5 \" `. J2 b' H) N8 r
10.   
    
11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
    0 J/ E; B, |9 F9 t
12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
    : c9 t' z2 I. |0 Q' Q
13.   
    
14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
    
15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    8 ]+ C7 P0 w- [5 L. g! e! {& R
16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
    3 W0 w9 Q# m+ `3 P& E
17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    , o, O; G% b$ H) H* Q; T
18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
      f& w  _( O2 r% `$ b: E+ y$ s
19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7
    8 r$ s7 M" R0 H6 A, D4 y. E
20. 
    
21.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    
22.   GPIO_Init(GPIOB,&GPIO_InitStructure);
    : s, C8 B4 p% P3 h
23.   
    
24.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
    
25.   GPIO_Init(GPIOC,&GPIO_InitStructure);
    
26.   
    
27.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
    ; q. o* v5 k1 w' n4 a/ q" y; i
28.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1
    
29.   GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
    9 d& v) d1 p* x$ B7 d( d
30.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13复用为定时器1
    & G4 |- s2 ^- @& x) }0 K- l
31.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14复用为定时器1
    
32.   GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15复用为定时器1
    . p. Q. |3 m1 [
33. 
    5 _7 W& c. C, h, R
34.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
      F* Q% g. F! W0 n6 h
35.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
36.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
    # i7 i& y: b9 ~8 a6 F) u
37.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
38.   
      g1 f2 {# |% S; T$ R
39.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
    
40.   
    
41.   //初始化PWM模式   
    ) `3 Z$ Z9 p; G1 T8 d
42.   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    
43.   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    
44.   TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    & z0 Z8 q7 w) v4 T6 z: r* V/ ^
45.   TIM_OCInitStructure.TIM_Pulse = 0;
    3 j" i9 R0 C! b7 P
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;
    # M6 l( W  _! h) p$ E- Y
50. 
    
51.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    
52.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    
53.   TIM_OC3Init(TIM8, &TIM_OCInitStructure);
    ' R: `: |( J: s- E( i
54. 
    % b# O+ K3 J$ N. _
55.   TIM_OC1PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
56.   TIM_OC2PreloadConfig(TIM8,TIM_OCPreload_Enable);
    4 Z- ^3 L; I; T% \# @- r
57.   TIM_OC3PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
58. 
    % A3 u: K' w2 f
59.   TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
      D0 ^- c7 {5 {. Q3 v! R7 ?
60.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
    
61.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
    ) G) c6 b; f/ |5 j
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;
    9 u  {% E% X  }; R) z" M
66.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);
    7 @5 g! J3 F( ~$ Y# Z
67. 
      k8 K* e5 |9 I; a3 W
68.   TIM_Cmd(TIM8,ENABLE);
    
69.   TIM_CCPreloadControl(TIM8,ENABLE);
    
70.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
    " `8 J5 n( t6 J" c7 h- w) C
71.                      
    
72. }</font>

复制代码

编码器初始化函数：
0 |9 A4 N" M3 f! u

1. <font face="微软雅黑" size="3">
   - r4 d1 S8 y: F8 s2 z0 u% u$ M
2. void Encoder_Init_TIM4(void)
   
3. {
   5 h) ]' a$ N% c
4.     GPIO_InitTypeDef         GPIO_InitStructure;
   0 ~# {7 P1 o% R3 \6 L
5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   
6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
   ; D9 m( \5 ?+ O) N
7.     NVIC_InitTypeDef NVIC_InitStructure;
   
8. 
   
9.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
   ' P& Y$ s0 U3 l; b7 m
10.     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟
    
11.   
    1 O: M6 Z$ e& f7 W9 o
12.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用
    ! O$ r& B1 _4 f6 k5 [' ^' A
13.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用
    
14. 
    ' m' [8 ]- M, L4 g( }$ ^
15.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
    ) B2 [! ?2 w8 C; ]1 C* L
16.     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
    ; I& v8 `; |6 o  X; }* v* M
17.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    
18.     GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    
19.     //GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL ;
    , ~+ `% P' u8 q9 g
20.     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
    * s$ S7 f5 q9 @) P3 P  W
21.     GPIO_Init(GPIOB,&GPIO_InitStructure);
    6 ?0 l* O% }$ f1 W3 P
22.    
    5 E) [6 _8 ?. u" {6 ]
23.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    * r1 t6 S+ o( x0 e/ N
24.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
    
25.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    
26.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断，防止计数溢出而没有相应函数，造成卡死
    
27.     NVIC_Init(&NVIC_InitStructure);
    ( a) m- e0 s+ W  d; X! y* h: ]+ N
28. 
    7 ]) C( i" r1 s& b, ~  V) f
29. 
    : b) X4 S# o" n: @5 l
30. 
    # |. L* V2 x0 n1 y# h/ n1 k8 o
31.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
      m9 P% U9 a) V+ e5 G3 m7 g; N0 P* c
32.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
    
33.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
    4 b) G1 C4 H8 g! y4 V0 b
34.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    
35.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    - I* `: \! D! {$ s
36. 
    : d9 B8 B6 \# H/ {2 V# A, P
37.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
    / b% ^, T  O: H( |% }4 Q0 E# o
38.     TIM_ICStructInit(&TIM_ICInitStructure);
    
39.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
    ' t. A5 b- T) n. s4 ^5 N
40.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
    * D5 d, o% h; O
41.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
    * |' W" U3 Y8 j# V4 V: O8 B
42.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
    
43.     TIM4->CNT = 0;
    + X4 }. |' ^& A
44.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
    
45. }
    
46. </font>

复制代码

编码器返回速度值：
$ I5 m# G1 `- M

1. <font face="微软雅黑" size="3">/**************************************************************************
   5 B: v- A' g7 W7 d  I8 T- U
2. 函数功能：单位时间读取编码器计数
   & W! }! J9 e$ s6 @  D2 I- @
3. 入口参数：定时器
   
4. 返回  值：速度值
   ( u: `6 D, D) u0 z8 g" V
5. **************************************************************************/
     l* [& X( f4 y5 s
6. float Read_Encoder_Speed(uint8_t TIMX)
   - E) h4 T, l. _  x3 X. F, U$ |7 }
7. {
   
8.     int32_t Encoder_TIM;
   $ T; {. F' I/ m" C
9.     float res = 0;
   7 L. i) A. s6 U1 Y) N$ r
10.     switch (TIMX)
    
11.     {
    
12.     case 5:
    # ~2 C) o5 q4 f6 }4 O: s0 {
13.         Encoder_TIM = TIM_GetCounter(TIM5);
    0 h/ }% _, o8 z, W0 M$ B$ |
14.         TIM5->CNT = ENCODER_BASE_COUNT;
    
15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    
16.         break;
    
17.     case 4:
    ) j( C  H) L5 S' z( c
18.         Encoder_TIM = TIM_GetCounter(TIM4);
    
19.         TIM4->CNT = ENCODER_BASE_COUNT;
    
20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    4 {, F8 T8 s% M4 y0 x
21.         break;
    * L# D: N% E* l1 h. O5 q. |
22.     default:
    / `+ O: v/ t' B" @, f  r
23.         Encoder_TIM = 0;
    , I3 o: W; B) v# s- b. b
24.         res = 0;
    6 u# u3 j! w" V* k* [
25.     }
    
26.     if(res>2048.0f)
    
27.         res-=4096.0f;
    - [- {. i7 t* \7 x0 g
28.     return res*360.0f/4096.0f;
    # T/ e4 m7 d- c$ L( o
29. }
    7 Z6 _7 f1 y- w
30. </font>

复制代码

定时从编码器取数，注意，时间不一样，取回的数值也不一样，取决于实际速度以及编码器线数。这里50ms取一次：
$ Z; b) V, `3 u4 I, B# k

1. <font face="微软雅黑" size="3">
   6 z" H" z3 m* ]
2. void TIM7_IRQHandler(void)//频率20HZ，用于编码器计数
   
3. {  
   
4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
   
5.   {
   " `" f  O6 Z7 ^9 e( j' }9 \) t; B
6.     speed=Read_Encoder_Speed(4);
   
7.   }
   
8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
   
9. }</font>

复制代码

2.5 PID控制
1 u& ?: b' p0 b1 i6 I6 n
# D8 T' C) }4 O$ d4 E; _6 {
* ^) \  G# |3 t1 JPID库函数：

  Y2 a* B) @9 T2 B

1. <font face="微软雅黑" size="3">
   ! ?5 Y- R& D2 x) X" b  q! R4 C
2. #define N 2        //需要对多少变量进行pid调节
     v0 I: E$ z( s
3. 
   # [" U" a' ^0 C7 |* v7 U
4. const float KP[N]={1.3,1.0};//这里只用了比例调节
   
5. const float KI[N]={0,0};
   
6. const float KD[N]={0,0};
   
7. 
   ; J4 f8 h+ ]2 I4 z: e5 g7 C3 r) d
8. 
   
9. struct _pid{
   
10.   float SetVol;        //定义设定值
    5 }/ N- U% R* Q9 P6 {
11.   float ActVol;        //定义实际值
    
12.   float Err;          //定义误差
    8 a" N" B! Z% ?! i0 V" k* N4 U
13.   float  Err_Next;      //定义上一个误差
    
14.   float  Err_Last;      //定义上上一个误差
    
15.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
    
16.   float integral;      //定义积分值
    
17.   float actuator;      //定义控制器执行变量
    
18. }pid[N];
    
19. 
    
20. void PID_Init(void)
    
21. {
    8 S5 ]1 ^/ R! }% h) Z; x, X" G: d
22.   for(int i=0;i<N;i++)
    
23.   {
    
24.     pid[i].SetVol=0.0;
    ) ~. V  X& ]& p/ \# S- g
25.     pid[i].ActVol=0.0;
    % W0 x7 \" E4 T' S  ?
26.     pid[i].Err=0.0;
    
27.     pid[i].Err_Next=0.0;
    
28.     pid[i].Err_Last=0.0;
    
29.     pid[i].integral=0.0;
    
30.     pid[i].actuator=0.0;
    
31.     pid[i].Kp=KP[i];
    
32.     pid[i].Ki=KI[i];
    
33.     pid[i].Kd=KD[i];
    ! x" B5 P0 m, a5 e! v; U* c, }
34.   }
    
35. }
    ( T& @6 B$ d; K' y% c* ]  a
36. 
    ' c, W7 M9 F) ^# k5 N! p
37. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
    6 N; }2 X+ Q- m* f/ E" ]* W3 U
38. {
    ! t& j. `/ y+ ?: N# F2 d7 D, k
39.   pid[i].SetVol=set_val;
    2 {  m+ U5 a: x6 q% l: r9 e1 M1 r
40.   pid[i].ActVol=get_val;
    
41.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
    
42.   float IncVol;    //定义增量
    # z' {+ U5 M9 C* \/ m
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);
    ! Q6 [  K9 ~3 ]3 g+ S; ^8 `
46. //  pid[i].actuator=adc_val+IncVol;
    
47.   pid[i].ActVol=pid[i].actuator;
    
48.   pid[i].Err_Last=pid[i].Err_Next;
    + e; G# T3 j& B& B/ h
49.   pid[i].Err_Next=pid[i].Err;
    
50.   
    
51.   return pid[i].actuator;
    2 t3 w, K* W; K# o  p
52. }</font>

复制代码

主函数中的PID调节：

: w8 o  ?7 m/ Q0 Z' J

1. <font face="微软雅黑" size="3">
   " c: ^* V" Q4 x5 f. n
2.     z_get=data[2];
   
3.     x_get=data[0];
   
4.     if(z_get-z_set>0.5||z_get-z_set<-0.5)//电机PID
   # }2 O6 a. i3 K: e3 |
5.     {
   
6.       LED1=0;  //调节时灯亮
   : w1 s' x: x2 S( W7 E
7.       PID_val_motor=PID_realize(z_set,z_get,0);
   
8.       PID_val_motor=PID_val_motor/10.0;
   " {9 Q  s7 k! X2 l4 A; P# {. H0 ]
9.       if(PID_val_motor<=0)
   
10.         motor_flag=0;//motor_flag控制电机正反转，PID_val_motor用于改变占空比，范围0~1
    
11.       if(PID_val_motor>0)
    $ T( G1 N5 A$ O- T6 R8 V
12.         motor_flag=1;
    : f3 Y" c8 Z% I- s# p& `) D+ G+ W/ H
13.       PID_val_motor=abs_float(PID_val_motor);
    + j6 L  r9 V' N7 @. w' N3 L
14.       if(PID_val_motor>2)PID_val_motor=0;//标志太远，让车停止
      }3 }/ a1 Z9 m% t3 @  B5 f) x
15.       if(PID_val_motor>1&&PID_val_motor<=2)PID_val_motor=1;
    
16.       if(PID_val_motor<0.2)PID_val_motor=0;
    
17.     }
    9 y) Q+ }  ?$ \( v! _
18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
    
19.     {
    
20.       LED1=0;  
    
21.       PID_val_servo=PID_realize(x_set,x_get,1);
    
22.       servo_angle=((140-35)/6)*PID_val_servo+35;//线性映射，把PID的值转化为角度35~140的舵机转角
    
23.       if(servo_angle<35)servo_angle=35;
    / H0 |+ }7 d: i- U
24.       if(servo_angle>140)servo_angle=140;
    
25.     }
    
26.     LED1=1;</font>

复制代码

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

1. <font face="微软雅黑" size="3">
   ( F; j6 j# ]7 n# `% Q8 }% D6 U
2. void TIM2_IRQHandler(void)
   6 I7 U* p, G/ j7 u( j
3. {  
   + G6 Z0 f. c& S. y
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   9 I. F1 J0 U% e9 j& S+ w% p( W  F
5.   {
   6 S7 A3 V( {& }+ \- |; p0 P
6.     if(motor_flag==1)//反转
   5 O3 r$ W! ^* @
7.     {
   , j; z- x8 s) }
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.     }
      P9 c) }# x3 \; G  C% s. w, @
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，根据舵机手册得到占空比与转角的关系，引脚：PA6
    " N3 w; Y& Z' X) V! l
17.   }
    9 T: v0 u% v8 w/ K3 b
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    
20. </font>

复制代码

/ V2 T$ H3 t, n& x# U* F
+ R/ o: p# S3 t6 X# a; t: {8 h1 Y
% v2 `# B& ^3 o2 _, T' N

优秀
2 y! `- [% O5 b/ `