概述
+ o2 R: t8 z0 c" |小车外形：


' A2 y; a* T7 p! C
功能简介
利用摄像头识别前车尾部的AprilTag，得到前车位置，传回stm32主控板处理，使两车在行驶时保持恒定距离，实现自动跟车。

* F7 u7 h2 B$ j$ F: _( n3 }

  }+ q! T7 E! H  r5 a
openMV4摄像头
, L3 a& b$ o5 Y1 t4 Y1.1 Apriltag识别与串口传输

' D; Z2 j1 y+ j9 g; y
AprilTag是一个视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志（与二维码相似，但是降低了复杂度以满足实时性要求），可以快速地检测标志，并计算相对位置。
4 o2 x+ T- d6 U

Apriltag示例：
% T) o5 O0 F. N* ^5 X
通过识别Apriltag，可以得到x,y,z三个方向的距离以及偏移角度。这里只需要三维的距离即可，通过串口传回stm32.

1. <font face="微软雅黑" size="3">
   
2. import sensor, image, time, math,pyb
   
3. from pyb import UART
   
4. 
   & L6 A; W/ s. D; L
5. sensor.reset()
   
6. sensor.set_pixformat(sensor.RGB565)
   % x6 a: e2 N* q7 Y
7. sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
   
8. sensor.skip_frames(time = 2000)
   # l7 d6 _+ a! h" p; z; j
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()
    7 K- G% W, O4 Z2 h  ~: X. T
12. uart = UART(3, 115200)#串口波特率
    
13. 
    7 q: B; v0 j% P
14. f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
    2 j7 q% [9 @; ]' e
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)
      w0 r/ i& O' z
17. c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
    
18. 
    
19. def degrees(radians):
    
20.     return (180 * radians) / math.pi
    
21. 
    
22. while(True):
    ; I$ p: q6 k* H; r  w: ]
23.     clock.tick()
    5 \5 c0 n6 Z. i, a& G* }% y$ n
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
    * ?- d" i2 ]# s( v1 n
26.         img.draw_rectangle(tag.rect(), color = (255, 0, 0))
    & j$ I6 r6 T0 g3 m$ d6 j. J7 ?! Z1 |/ b
27.         img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
    
28.         print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
    . E. @* ^5 H4 ]8 Y
29.             #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
    ) ]4 A9 }) z0 ^' @; f( {* G
30.         # Translation units are unknown. Rotation units are in degrees.
    * J/ k! j$ u7 V- j$ O- e# b1 `7 m8 Y; w
31.        # print("Tx %f, Ty %f, Tz %f" % print_args)
    ) T% G/ `: h# S0 K; V  C1 D
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）
' _' y9 `  W8 }( T6 O0 p- t$ L2 ^/ P
% L% A- w+ v, H9 R' T4 o
7 S) W; G7 \2 U- T  g+ y0 c2.1 时钟与中断配置
8 O+ K# O8 B" P& D9 ^3 G

& E- [. Z" P: [附上stm32时钟示意图：
0 A' s: u! Z* y6 R
定时器示意图：
1 _' t7 |. @+ X0 I! A; R
定时器分配：

所有时钟初始化的函数：（每个函数的详细内容在后面）
6 A' Y: l+ r$ Z  f+ I! C7 r; f+ [

1. <font face="微软雅黑" size="3">
   
2. TIM8_PWM_Init(400-1,20-1);  //用于控制电机，168M/20=8.4Mhz的计数频率,重装载值400，所以PWM频率为 8.4M/400=21Khz.
   0 s7 w2 S5 n8 n, A$ S: }7 n% C/ D; @
3.   TIM3_PWM_Init(200-1,8400-1);//用于控制舵机，50HZ
   7 a& j9 k4 m4 A" i4 S: C
4.   TIM2_Int_Init(400-1,20-1);//定时中断，21KHZ
   
5.   TIM7_Int_Init(500-1,8400-1);//用于编码器计数，20HZ，50ms中断一次
   ! V6 b6 P7 p; F. m" S
6.   uart_init(115200);  //初始化串口1波特率为115200
   $ h4 c, K' \7 X7 ~* l- G
7.   Encoder_Init_TIM4();//编码器接口初始化</font>

复制代码

2.2 串口收发与数据处理


; a! _7 o6 Z, Y+ v+ L# D串口中断：USART1，USART2
4 F% _1 k2 A6 e  i  A, h串口初始化函数（以USART1为例）：

1. <font face="微软雅黑" size="3">
   # Y$ r5 k, w1 G- N" x
2. void uart_init(u32 bound){
   ! z+ e: b& J/ O7 U$ p/ s* C4 b% W# v
3.    //GPIO端口设置
   ; u) y" [) W3 F- D
4.   GPIO_InitTypeDef GPIO_InitStructure;
   
5.   USART_InitTypeDef USART_InitStructure;
   
6.   NVIC_InitTypeDef NVIC_InitStructure;
     `' h9 H% r7 d3 e5 `9 v
7.   
   
8.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
   
9.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟
   * {! v7 `+ @' F& e
10. 
    0 y2 l; x: A7 Z8 L
11.   //串口1对应引脚复用映射
    
12.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1
    . G/ ?% Z) M8 ~' j5 X$ P9 n. `0 M. [
13.   GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
    + l' V0 ^" @( l% K2 P8 g6 r
14.   
    . V* g) v( f  s1 C
15.   //USART1端口配置
    ! k' t. A* m+ i" }6 D& ?& {
16.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
    
17.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    9 m5 |7 Q& a7 M: X% p: r5 b
18.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度50MHz
    . t1 \" K6 p% ?9 L: ~5 ?7 j
19.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
    
20.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
    
21.   GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9，PA10
    3 V3 f3 X5 w" u9 k/ G
22. 
    , X4 k. q. T* V& u1 s; p$ P
23.    //USART1 初始化设置
    ; `% v' \% {. v- D
24.   USART_InitStructure.USART_BaudRate = bound;//波特率设置
    ' X% p" X% x' V9 l0 ?  ?
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;//无奇偶校验位
    $ _* z6 d- Z( Q
28.   USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
    
29.   USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;  //收发模式
    9 y+ l: u/ S/ g$ g# Q* B
30.   USART_Init(USART1, &USART_InitStructure); //初始化串口1
    - O/ Y* b9 Q6 o7 B& H
31.   
    $ {  K/ o  Q% T- ^
32.   USART_Cmd(USART1, ENABLE);  //使能串口1
    ! \, \5 J. s, }1 }# }# d
33.   
    
34.   USART_ClearFlag(USART1, USART_FLAG_TC);
    
35.   
    + W( K/ {. J/ _: j
36. #if EN_USART1_RX  
    * |- P. j/ f8 N' n
37.   USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断
    , ]! s" n  L( `7 x" G" g1 F( ]
38. 
    
39.   //Usart1 NVIC 配置
    / ^1 w3 d' m8 z( G
40.   NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道
    
41.   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
    
42.   NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;    //子优先级3
    + c" i. y/ a5 ?
43.   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //IRQ通道使能
    . z/ M8 X, g( L+ u8 A9 U
44.   NVIC_Init(&NVIC_InitStructure);  //根据指定的参数初始化VIC寄存器、
    3 q. V4 ^9 O, o7 E; v4 m! l
45. 
    8 S- P, h5 |  o. z
46. #endif
    
47.   
    6 V( D% e4 U" {8 L& _
48. }</font>

复制代码

串口中断处理函数：

1. <font face="微软雅黑" size="3">
   
2. void USART1_IRQHandler(void)                  //串口1中断服务程序
   + @7 O4 Q1 W# u( @2 B
3. {
   
4.   u8 Res;
   
5. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
   $ s2 S6 x4 K3 Q3 V
6.   OSIntEnter();   
   
7. #endif
   
8.   if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
   0 H6 Z% D7 t6 U0 S7 b6 f
9.   {
   
10.     Res =USART_ReceiveData(USART1);//(USART1->DR);  //读取接收到的数据
    , N( _  e+ B7 t7 ?
11.    
    
12.     if((USART_RX_STA&0x8000)==0)//接收未完成
    
13.     {
    & n" M2 P9 Z# d. q# i+ [4 `6 V
14.       if(USART_RX_STA&0x4000)//接收到了0x0d
    
15.       {
    & `" V6 P! ^! x6 U
16.         if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
    . J# t% C; z  A6 R1 q
17.         else USART_RX_STA|=0x8000;  //接收完成了
    
18.       }
    
19.       else //还没收到0X0D
    # G* Y! b* _! n! |7 |; w* Z/ ~8 ~
20.       {  
    
21.         if(Res==0x0d)USART_RX_STA|=0x4000;
    4 W) F0 C" G6 y- y( x# i) ]3 i
22.         else
    ( U0 z# @; u) V, v
23.         {
    
24.           USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;
    2 s. i% w# ?" X$ e- {
25.           USART_RX_STA++;
    
26.           if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收   
    % C" H7 h+ C, s! @, B$ z
27.         }     
    * `: p( {6 u' Y9 N
28.       }
    
29.     }        
    
30.   }
    
31. #ifdef OS_TICKS_PER_SEC     //如果时钟节拍数定义了,说明要使用ucosII了.
    
32.   OSIntExit();                        
    . Y9 X/ u; Q9 \* m: K
33. #endif
    % P' m7 N2 c8 g- q5 t- v
34. }
    3 f) K, h% v4 y* R- G+ ?) r
35. #endif  </font>

复制代码

字符串接收与处理（从openMV接收到的数据）：
/ B5 X% Z9 ^3 Q

1. <font face="微软雅黑" size="3">/*涉及到的全局变量
   
2. float data[3];//x,y,z方向的距离，浮点数形式
   
3. unsigned char data_string[3][7];//x,y,z方向的距离，字符串形式
   
4. */
     M- q2 o, I: [3 i1 ]4 W% {
5. if(USART_RX_STA&0x8000)
   
6.     {  
   4 K$ }6 k6 ]; p
7.       //清空字符串
   , R- U4 L2 k" n2 o! F
8.       for(i=0;i<2;i++)
   3 e. U5 W! e$ z* V; Y
9.       {
   
10.         for(j=0;j<6;j++)
    " H' T. g5 w& q3 S. G
11.         {
    
12.           data_string[i][j]=' ';
    ! c( }0 {" M& S; L* ?
13.         }
    
14.       }
    
15.       len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度
    . N) u3 o5 l" S+ r8 I6 q
16.       for(t=0,j=0;t<len;t++)
    
17.       {
    * y4 `0 U+ V: ~: N+ y2 W8 y
18.         j=0;
    / z/ Y" K5 |( l2 {) v
19.         if(USART_RX_BUF[t]=='A')
    
20.         {
      n" b+ J) b* \6 @* u8 z
21.           t++;//去掉首字母
    
22.           while(USART_RX_BUF[t]!=',')
    
23.           {
    
24.             data_string[0][j]=USART_RX_BUF[t];
    
25.             t++;
    
26.             j++;
    " `) Y: O8 s% H6 R) B
27.           }
    
28.         }
    1 S/ c- s7 w! D( z; H
29.         if(USART_RX_BUF[t]=='B')
    
30.         {
    
31.           t++;//去掉首字母
    ( f, _! d' ~3 z0 F' c
32.           while(USART_RX_BUF[t]!=',')
    : }3 d# _+ o" Q* a
33.           {
    * s/ X+ I) t7 Q
34.             data_string[1][j]=USART_RX_BUF[t];
    
35.             t++;
    0 z# _: g% ?! @) i6 v, a
36.             j++;
    ' ?  G  j" b  @( f
37.           }
    . H! U) M( T$ l: Z. s3 P
38.         }
    5 d7 w/ B1 _  P& f8 c. \+ ?
39.         if(USART_RX_BUF[t]=='C')
    % O4 R5 ~  s: t' F6 i" S
40.         {
    , k+ n2 {7 @$ A  ?+ Y9 y
41.           t++;//去掉首字母
    / |9 N. x9 |) A
42.           while(USART_RX_BUF[t]!=',')
    
43.           {
    " E9 m+ o  Z7 X
44.             data_string[2][j]=USART_RX_BUF[t];
    , K2 X" A+ B& U4 ?" _
45.             t++;
    
46.             j++;
    
47.           }
    ; R$ @4 [: p' ~+ \
48.         }
    # x; ]: ~' s3 ~! o- E) H+ X5 f& c
49.       }
    
50.       //把字符串转化为浮点数
    
51.       data[0]=myatof(data_string[0])/100.0;//x
    
52.       data[1]=myatof(data_string[1])/100.0;//y
    1 E# x6 |! o8 z: i9 n2 R% ]
53.       data[2]=myatof(data_string[2])*(-1)/100.0;//z，输入是负数，转换为正
    
54.         //USART2发送数据
    
55. //        Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
    0 z4 |+ s" P% ^) p. u! P; m
56.         
    
57.       //LCD更新显示
    
58.       //显示xyz
    
59. //        CLEAR(10);
    1 v1 |% }! h2 \; g
60. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);
    2 l- F) [4 U+ A, q( G
61. //        Gui_DrawFont_GBK16(30,0,BLACK,WHITE,data_string[0]);
    
62. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,clear);
    
63. //        Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
    
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;//清除标志位
    
68.     }
    
69. }
    - N. i4 N6 Z1 z4 `3 T
70. </font>

复制代码

字符串转化为两位小数浮点数（用于后续PID控制）:
2 E& r* ^4 Y9 ^  \6 t5 |  ]3 d
* }' U& I( e, l) B' r  J

1. <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数，返回的是乘100后的int值，因float返回有错误
   " f$ m% F6 y, b; g& K3 K
2. {
   % v# y* e  K& c# u
3.   int flag = 1;//表示正数
   
4.   int res =0;
   # N  k. {0 r$ ]  k3 ?
5.   u8 i=1; //小数点后两位
   
6.   while(*str != '\0')
   
7.     {
   
8.       if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字
   
9.       {
   : ~5 S) h! i% s$ j0 \  [
10.         str++;
    
11.         continue;
    6 U3 T* Y% ]+ Q2 S$ T4 n. ]
12.       }
    4 H: d; |6 |# H" D) O
13.       if(*(str-1) == '-')
    
14.       {
    
15.         flag=-1;//表示是一个负数
    ! f0 ~5 K9 D3 J. J9 a
16.       }
    9 `$ A1 K# ^  s  R( l9 T
17. 
    
18.     while(*str >= '0' && *str <= '9')
      G) S5 \  u/ d
19.     {
    1 Q- x6 s, a( u) L5 X
20.       res = res *10 + (*str - '0');
    * ~' U) o  l# Q3 Z0 f8 j4 N/ P
21.       str++;
    ! L4 D* D* t, p; i" `, Q2 b# P) Q/ y
22.     }
    
23.     if(*str == '.')
    9 T' l# J# \/ f. k$ M: n
24.     {
    
25.       str++;
    
26.       res = res *10 + (*str - '0');
    % ?# c9 j$ N  O1 ^6 [" a
27.       str++;
    
28.       res = res *10 + (*str - '0');//保留两位，故加两次
    % g3 E) B) h! A8 n: B/ m- @
29.       return res*flag;
    
30.     }
    
31.     }
    % ^' W* N2 ^% O
32. }
    6 O% D) i- E; B4 W' g9 N
33. 
    2 Y1 n  x) O. Y# z4 _
34. 
    
35. 2.3 LCD显示模块
    
36. 
    9 h. @" ?4 h5 U2 g+ U9 ~
37. LCD模块用于调试时观察数据，调试完成可以删去，因为显示屏很耗时，使处理速度变慢
    * t! ~) s( |& d) P2 J( v6 [2 u
38. 驱动函数总览：
    
39. 
    : u9 g/ I% ?, y2 n
40. 
    ' ]+ L7 P0 M! v# ]+ r; `+ l
41. 
    
42. void LCD_GPIO_Init(void);
    2 f4 c$ G' F. @! K  z6 `
43. void Lcd_WriteIndex(u8 Index);
    9 v7 F# {# R! P! Z
44. void Lcd_WriteData(u8 Data);
    
45. void Lcd_WriteReg(u8 Index,u8 Data);
    ' ^1 u$ @6 E! G( ^, u$ Z; p( s# j/ i. }
46. u16 Lcd_ReadReg(u8 LCD_Reg);
    5 S- K2 t/ R6 u
47. void Lcd_Reset(void);
    - q8 @9 t9 W( Y. y( w8 U8 x
48. void Lcd_Init(void);
    
49. void Lcd_Clear(u16 Color);
    
50. void Lcd_SetXY(u16 x,u16 y);
    
51. void Gui_DrawPoint(u16 x,u16 y,u16 Data);
    
52. unsigned int Lcd_ReadPoint(u16 x,u16 y);
    ( A: K" F  [) D+ N1 W
53. void Lcd_SetRegion(u16 x_start,u16 y_start,u16 x_end,u16 y_end);
    3 c" E+ {6 m2 B4 B/ m
54. void LCD_WriteData_16Bit(u16 Data);</font>

复制代码

TFT屏幕初始化：

" D3 Q3 P3 R) x( }* |

1. <font face="微软雅黑" size="3">void TFT_Init_Show(void)
   2 `! O( o' {( V$ a% ]4 k1 K
2. {
   
3.   Lcd_Clear(WHITE);
   : Z6 z0 ]9 w: }+ S  P
4.   Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");
   
5.   delay_ms(1000);
   0 d4 g7 s7 T+ r: {
6.   Lcd_Clear(WHITE);
   - X- L( \5 j& t/ H* _
7.   Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
   6 D3 t0 ^/ \7 ~6 U2 |& n) a8 p- M
8.   Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:");
   1 y5 @0 G/ @/ ~0 o
9.   Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:");
   
10.   Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");
    2 y" @' n* R& A* k3 y
11. }</font>

复制代码

字符串显示函数;

1. <font face="微软雅黑" size="3">
   
2. void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
   
3. {
   
4.   unsigned char i,j;
   ) q$ S( p5 U4 e0 Y7 s
5.   unsigned short k,x0;
   : U  R! s; _" l! G9 k
6.   x0=x;
   
7. 
   * X1 f% X8 x* C' c
8.   while(*s)
   8 Q7 g& |  A" V; |: p. t
9.   {  
   
10.     if((*s) < 128)
    
11.     {
    
12.       k=*s;
    
13.       if (k==13)
    
14.       {
    7 U. k0 U$ i( d  G% g8 x% T# J
15.         x=x0;
    , ^1 b  o/ x3 R2 d
16.         y+=16;
    
17.       }
    % B! Q6 |5 g/ V/ C+ x& T, S5 ^
18.       else
    0 ~, w: h, w1 H0 l/ a$ s
19.       {
    
20.         if (k>32) k-=32; else k=0;
    , @1 g6 S. Q: X) t; S- f
21.   
    $ |1 ]6 C  r9 H. r
22.           for(i=0;i<16;i++)
    
23.         for(j=0;j<8;j++)
    8 m$ f9 c! I! T3 o# o5 ]
24.           {
    
25.               if(asc16[k*16+i]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    
26.             else
    ( t* G7 g+ t6 k: x4 D
27.             {
    
28.               if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    
29.             }
    
30.           }
    ( f, d% J7 F0 @0 x% W" B1 a
31.         x+=8;
    
32.       }
    
33.       s++;
    9 _$ v  M. O5 v: n+ g
34.     }
    9 V* ^* _4 e1 D) J) ~6 Z; V1 _
35.       
    & Y- A" j# c' D8 C" b1 {
36.     else
    & X. H5 z1 g* C4 O! n
37.     {
    
38.    
    7 m, a3 |! [7 n! J
39. 
    
40.       for (k=0;k<hz16_num;k++)
    + z  c6 t5 @. g+ i6 q& u7 Q3 j
41.       {
    
42.         if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
    
43.         {
    4 X; F+ u/ v1 {' {' E% H. ?
44.             for(i=0;i<16;i++)
    
45.             {
    8 a9 k2 a( U! U+ \3 J. f
46.             for(j=0;j<8;j++)
    4 I* d1 c! c( V
47.               {
    
48.                   if(hz16[k].Msk[i*2]&(0x80>>j))  Gui_DrawPoint(x+j,y+i,fc);
    1 N. v: ]/ b9 ~  Q0 `
49.                 else {
    
50.                   if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
    2 C' {1 }3 T7 q" I9 t
51.                 }
    
52.               }
    
53.             for(j=0;j<8;j++)
    & ~" e4 v: Q7 [4 r
54.               {
    5 s9 [3 c4 H9 `2 p, @; {& e
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);
    
59.                 }
    % r& V2 ^  A7 P/ U( ^- b8 G2 K8 R
60.               }
    
61.             }
    ) \# C% q' L/ Q  h# v, |
62.         }
    
63.         }
    * e+ o: V! a/ y" I7 s" e$ t. l4 u
64.       s+=2;x+=16;
    
65.     }
    
66.    
    . z7 H, F1 o$ e# J" T
67.   }
      x2 g6 O* u, l* x+ e5 Y
68. }
    
69. </font>

复制代码

2.4 电机、舵机与编码器


2 Z3 v3 G9 q7 q& V定时中断：TIM2，用于修改电机和舵机的PWM占空比
初始化函数：
5 O& q* f2 g' B3 |" l
" Z9 _% |5 h3 H

1. <font face="微软雅黑" size="3">
   3 f5 b* U! [1 }# P/ V
2. //通用定时器2中断初始化
     I- o% }, S8 {2 p, Y
3. //arr：自动重装值。
   9 Z5 @  Y5 X4 G- [0 _, V
4. //psc：时钟预分频数
   + a; i. c/ W& u) K" t3 ~, O" d
5. //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
   
6. //Ft=定时器工作频率,单位:Mhz
   
7. //这里使用的是定时器2!
   
8. void TIM2_Int_Init(u16 arr,u16 psc)
   
9. {
   ; f0 g, R' h2 I1 }
10.   TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    
11.   NVIC_InitTypeDef NVIC_InitStructure;
    3 D: m$ T4 h$ p9 k4 @- Z* n, |
12.   
    % ?% Y6 Q( k4 B, g1 s) j
13.   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);  ///使能TIM2时钟
    1 a: V. x$ Y1 j- R6 U, U
14.   
    - S; C. E; W7 c
15.   TIM_TimeBaseInitStructure.TIM_Period = arr;   //自动重装载值
    4 l4 I# o9 ?: w1 {
16.   TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
      N& {5 B9 ^, ]2 A; w% n. v
17.   TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
18.   TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
19.   
    9 R) F1 E8 m0 L* e; Q) _7 u8 }
20.   TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3
    
21.   
    
22.   TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断
    7 C  s1 |" n4 }$ ]9 F: D; }
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
    ) q  S5 e1 f% @5 g6 B
28.   NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
      t3 V+ f/ J" Z
29.   NVIC_Init(&NVIC_InitStructure);
    + t9 F" W9 B5 ~% ~& Z. c6 ^
30. 
    7 S' Q9 [# U5 C* s, r
31. }</font>

复制代码

TIM2中断处理函数：
# J( C/ ]7 M5 P: k  p% l( f2 ?* j

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.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   
9.       TIM_SetCompare2(TIM8,0);
   
10.     }
    
11.     if(motor_flag==0)//正转
    , g; d; g/ ~* ?+ D( O
12.     {
    
13.       TIM_SetCompare1(TIM8,0);
    
14.       TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比，引脚：PC7
    
15.     }
    5 W2 E. u' M1 A0 D1 V
16.     TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度，引脚：PA6
    
17.   }
    
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    ' i( X2 |5 z7 A# y( V
20. </font>

复制代码

PWM输出：TIM3（舵机），TIM8（电机）
" ~5 |; Y2 t- T/ h- u0 K1 a' z7 A初始化函数（以TIM8为例）：
' q8 W& F! x& e% b9 B0 g
: l# ?7 W; ^0 Y5 \! i' K6 {

1. <font face="微软雅黑" size="3">
   ' E# i) J# y0 r, ]! ]/ c- w8 {
2. void TIM8_PWM_Init(u32 arr,u32 psc)
   / R! x- @' ^# i
3. {               
   
4.   //此部分需手动修改IO口设置
   
5.   
   
6.   GPIO_InitTypeDef GPIO_InitStructure;
   $ a7 H/ o, r+ x& X" a3 j! Z; h% A
7.   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   
8.   TIM_OCInitTypeDef  TIM_OCInitStructure;
   1 I7 w8 n/ _+ Y6 k3 z8 \
9.   TIM_BDTRInitTypeDef  TIM_BDTRInitStructure;
   
10.   
    9 W% u9 p  }7 `7 y
11.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);    //TIM8时钟使能   
    
12.   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE);   //使能PORTA时钟  
    ! D4 O/ P9 D0 U5 _  L* j' `
13.   
    7 Y- B0 @6 {- B, L, ^
14.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;           //GPIOFA
    . Y# m; Z3 n1 ~$ k
15.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    
16.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  //速度100MHz
    - l5 k8 X" J7 v$ m8 ?
17.   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    
18.   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
    0 n+ E1 O1 f; ]$ A
19.   GPIO_Init(GPIOA,&GPIO_InitStructure);              //初始化PA7
    
20. 
    6 Z. f1 t  a( M# z% J4 P! e- Y
21.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
    
22.   GPIO_Init(GPIOB,&GPIO_InitStructure);
    
23.   
    ( s: P) R, B* M' t% _) o1 ^
24.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
    
25.   GPIO_Init(GPIOC,&GPIO_InitStructure);
    1 T, M1 m$ D! T! P
26.   
    
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
    
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
    
33. 
    
34.   TIM_TimeBaseStructure.TIM_Prescaler=psc;  //定时器分频
    / V/ \! f- O% Y9 t
35.   TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    
36.   TIM_TimeBaseStructure.TIM_Period=arr;   //自动重装载值
    
37.   TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    
38.   
    % P7 U0 L0 U; f# f2 {
39.   TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1
    & e6 y% |) J! r1 D2 i7 B% e
40.   
    
41.   //初始化PWM模式   
    7 I  D# g1 ~( ^0 p& T
42.   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    " D4 g4 `& H. T  \' Q
43.   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    
44.   TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
    
45.   TIM_OCInitStructure.TIM_Pulse = 0;
    6 B- b' a4 D% F* v$ x
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;
    
50. 
    
51.   TIM_OC1Init(TIM8, &TIM_OCInitStructure);
    4 m- ]  C! k6 r; a" F! e
52.   TIM_OC2Init(TIM8, &TIM_OCInitStructure);
    7 @" O( v9 _- Z8 E5 q: R& h) x
53.   TIM_OC3Init(TIM8, &TIM_OCInitStructure);
    
54. 
    : J! l. W. A/ S; U
55.   TIM_OC1PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
56.   TIM_OC2PreloadConfig(TIM8,TIM_OCPreload_Enable);
      l  J: G! v' W  q) B- L2 l
57.   TIM_OC3PreloadConfig(TIM8,TIM_OCPreload_Enable);
    
58. 
    
59.   TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
    
60.   TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
    7 F! B' T9 K7 a
61.   TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
    
62.   TIM_BDTRInitStructure.TIM_DeadTime = 0;
    7 f0 F0 t9 I! s$ U) L
63.   TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
    
64.   TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;
    4 X" U( j( T( z
65.   TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
    ! `& |+ x, Z4 [$ W5 a3 V
66.   TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);
    $ E7 T" i1 ]0 n
67. 
    
68.   TIM_Cmd(TIM8,ENABLE);
    
69.   TIM_CCPreloadControl(TIM8,ENABLE);
    
70.   TIM_CtrlPWMOutputs(TIM8,ENABLE);
    
71.                      
    
72. }</font>

复制代码

编码器初始化函数：
, a* z4 C$ X& P% y- k! \

1. <font face="微软雅黑" size="3">
   % p/ Q1 u, z0 n) G0 k9 l
2. void Encoder_Init_TIM4(void)
   $ c1 N+ b+ t& C/ L, J3 K) i2 R$ g
3. {
   
4.     GPIO_InitTypeDef         GPIO_InitStructure;
   
5.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   
6.     TIM_ICInitTypeDef        TIM_ICInitStructure;
   
7.     NVIC_InitTypeDef NVIC_InitStructure;
   ) D) ]) m  H1 I# W, n
8. 
   
9.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
   
10.     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟
    
11.   
    
12.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用
    ' e2 L0 V% [$ r' s, L
13.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用
    
14. 
    ! V: p9 |: `5 \- L
15.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
    + `" Q+ u5 s- P8 _( t* Q
16.     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
    
17.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    
18.     GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
    * t( t6 a' _2 T2 U  \9 c
19.     //GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL ;
    / x7 ~. g4 B8 p
20.     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
    
21.     GPIO_Init(GPIOB,&GPIO_InitStructure);
    ' X, ~1 N( Z6 b' o4 s/ g
22.    
    
23.     NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    
24.     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/
    5 x5 ~% q8 e+ p$ G
25.     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    
26.     NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断，防止计数溢出而没有相应函数，造成卡死
    
27.     NVIC_Init(&NVIC_InitStructure);
    3 l& E& }4 ~! X' [$ v/ q
28. 
    
29. 
    2 u+ B: m9 v) s  ]9 B& M
30. 
    6 Z- ~: Y9 V  y9 H9 n! c* b3 R4 d$ `
31.     TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
    
32.     TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值  不分频
    
33.     TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
      b; E5 R/ s% d" _+ i' X
34.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
    ' M% `6 s! [! g/ L( C
35.     TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    + M7 @# l7 c1 e/ z7 X$ I
36. 
    
37.     TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
    # ~4 ~! L! ]3 K
38.     TIM_ICStructInit(&TIM_ICInitStructure);
    
39.     TIM_ICInitStructure.TIM_ICFilter = 10;  //输入滤波器
    ' f& q+ G3 ~, _; l* u* `# W
40.     TIM_ICInit(TIM4, &TIM_ICInitStructure);
    
41.     TIM_ClearFlag(TIM4, TIM_FLAG_Update);  //清除所有标志位
    3 F8 ~/ D0 r2 Q( [8 j  r# M
42.     TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
    
43.     TIM4->CNT = 0;
    6 G2 U1 f% F1 t; i) a- R8 F
44.     TIM_Cmd(TIM4, ENABLE);  //使能TIM4
    9 }. x" ~" j; O: s) V
45. }
    & }# m" K8 E+ H
46. </font>

复制代码

编码器返回速度值：
' ?9 m- N7 h- g1 q

1. <font face="微软雅黑" size="3">/**************************************************************************
   4 A9 [$ ?2 d. ]# n" F- g; u  y( o2 j
2. 函数功能：单位时间读取编码器计数
   : L: X8 T# F. K4 \1 `
3. 入口参数：定时器
   
4. 返回  值：速度值
   $ r/ J; j7 O) j# v( L) }
5. **************************************************************************/
   ( g7 i7 S2 d8 V5 H( T
6. float Read_Encoder_Speed(uint8_t TIMX)
   
7. {
   2 @# ]: Q* T0 ?) z  n) ^# ~
8.     int32_t Encoder_TIM;
   , y  I1 e5 K! j, i2 J
9.     float res = 0;
   : y" @2 Z  m# R5 m& J" s
10.     switch (TIMX)
    6 N$ Z  W4 d" m$ a4 {  c
11.     {
    9 T: i1 {! K6 M+ O- G& u" X; m6 c1 D
12.     case 5:
    : Z! ?1 Y3 r- M0 f
13.         Encoder_TIM = TIM_GetCounter(TIM5);
    + I" m* J; S( @- S# |& U/ Z
14.         TIM5->CNT = ENCODER_BASE_COUNT;
    : |9 ?5 e! s3 m1 _) k9 B4 r- m
15.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    0 B8 |( W- N& ?3 U
16.         break;
    
17.     case 4:
    
18.         Encoder_TIM = TIM_GetCounter(TIM4);
    ! l3 B; N# X! ^: p3 _$ {
19.         TIM4->CNT = ENCODER_BASE_COUNT;
    
20.         res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
    ) d8 V; J7 t& l4 x# k0 b. f
21.         break;
    . P! C! ~( \, T
22.     default:
    
23.         Encoder_TIM = 0;
    
24.         res = 0;
    
25.     }
    
26.     if(res>2048.0f)
    
27.         res-=4096.0f;
    
28.     return res*360.0f/4096.0f;
    
29. }
    
30. </font>

复制代码

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

1. <font face="微软雅黑" size="3">
   # O5 F5 H6 Q# U; s0 }+ {2 Y6 W
2. void TIM7_IRQHandler(void)//频率20HZ，用于编码器计数
   
3. {  
   
4.   if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
   $ v! f& u) V0 p, i; Z$ A8 ~3 h
5.   {
   : ?1 M0 U2 R" S8 R! O
6.     speed=Read_Encoder_Speed(4);
   
7.   }
   8 H" q4 \; k4 }3 R9 a1 Y% J% d1 ?0 U
8.   TIM_ClearITPendingBit(TIM7,TIM_IT_Update);  //清除中断标志位
   
9. }</font>

复制代码

2.5 PID控制
2 W  r5 f- T% y+ k, A% ?/ k6 `

PID库函数：

1. <font face="微软雅黑" size="3">
   
2. #define N 2        //需要对多少变量进行pid调节
   0 w& h2 `6 U0 l5 y' G% x0 Y
3. 
   
4. const float KP[N]={1.3,1.0};//这里只用了比例调节
   
5. const float KI[N]={0,0};
   
6. const float KD[N]={0,0};
   % [1 ]5 O% ]! J5 O
7. 
   4 H8 \( D% j" |7 [- s
8. 
   - u8 I' \& D' O: w- d/ U$ [
9. struct _pid{
   
10.   float SetVol;        //定义设定值
    
11.   float ActVol;        //定义实际值
    
12.   float Err;          //定义误差
    
13.   float  Err_Next;      //定义上一个误差
    8 J- @/ ?, h4 Z% J' x0 r5 w
14.   float  Err_Last;      //定义上上一个误差
    
15.   float Kp,Ki,Kd;      //定义比例、积分、微分系数
    
16.   float integral;      //定义积分值
    
17.   float actuator;      //定义控制器执行变量
    6 R" ]0 i, l! i$ j! U
18. }pid[N];
    
19. 
    # {4 P; }; a% s) X* O& }; n) A/ o
20. void PID_Init(void)
    / g! Y- n0 n; g1 u# e! q' o
21. {
    
22.   for(int i=0;i<N;i++)
    
23.   {
    ) I+ i/ ?* ]8 F; u# u: u% }) v- c
24.     pid[i].SetVol=0.0;
    / Q1 i. m. N( L7 F' Z( @1 t
25.     pid[i].ActVol=0.0;
    : {2 ~: U2 D" n- }* Q4 a9 c
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];
    % L( v1 F) `, w9 d* u
32.     pid[i].Ki=KI[i];
    . R8 L' s! E: D" c1 M) e! T0 \
33.     pid[i].Kd=KD[i];
    / y0 j$ A4 n8 M5 `* J
34.   }
    
35. }
    
36. 
    
37. float PID_realize(float set_val,float get_val,int i)      //位置型PID算法实现
    3 f% Z! z1 a1 H! }' l% z
38. {
    ( B: _. R9 d4 E* I6 e% i
39.   pid[i].SetVol=set_val;
    
40.   pid[i].ActVol=get_val;
    
41.   pid[i].Err=pid[i].SetVol-pid[i].ActVol;
    9 L6 S. q0 o3 z: }, j; _
42.   float IncVol;    //定义增量
    . i. F$ l8 m* v3 R" N
43.   pid[i].integral+=pid[i].Err;
    9 H! U2 k2 I3 j( w
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);
    : P8 r7 S$ A" v8 e
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;
    
47.   pid[i].ActVol=pid[i].actuator;
    8 e4 Z9 j' U8 p9 V
48.   pid[i].Err_Last=pid[i].Err_Next;
    - X, i2 }2 I2 P- w6 l
49.   pid[i].Err_Next=pid[i].Err;
    - {9 B- O3 M- b0 R7 z# ]  L
50.   
    
51.   return pid[i].actuator;
    + ~+ o8 t5 N" X) r8 T, j
52. }</font>

复制代码

主函数中的PID调节：

* m1 v4 R% U& p/ N

1. <font face="微软雅黑" size="3">
   
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
   + Z" S- h. I% _. L' j
5.     {
   
6.       LED1=0;  //调节时灯亮
   . {8 G) @4 T  A2 H6 C! |5 G4 }; p
7.       PID_val_motor=PID_realize(z_set,z_get,0);
   
8.       PID_val_motor=PID_val_motor/10.0;
   5 h0 _2 a5 Z6 j. h) S
9.       if(PID_val_motor<=0)
   " d& z: N; ~4 Y0 }6 @6 z( @
10.         motor_flag=0;//motor_flag控制电机正反转，PID_val_motor用于改变占空比，范围0~1
    
11.       if(PID_val_motor>0)
    6 ?. B% e& w" R" x9 Z8 R7 T4 F
12.         motor_flag=1;
    
13.       PID_val_motor=abs_float(PID_val_motor);
    * n1 t/ o! n9 j+ S5 @4 A
14.       if(PID_val_motor>2)PID_val_motor=0;//标志太远，让车停止
    - T5 ?  k7 s, G- {5 ]$ w5 z: h
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.     }
    6 G+ M9 h9 N1 X* \, b+ j' e
18.     if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID
    
19.     {
    
20.       LED1=0;  
    2 |4 K4 f0 [7 y0 D4 A( j/ d
21.       PID_val_servo=PID_realize(x_set,x_get,1);
    
22.       servo_angle=((140-35)/6)*PID_val_servo+35;//线性映射，把PID的值转化为角度35~140的舵机转角
    ; K9 R! f8 u4 i2 o9 `: F
23.       if(servo_angle<35)servo_angle=35;
    
24.       if(servo_angle>140)servo_angle=140;
    
25.     }
    / E( s0 L" O# ^
26.     LED1=1;</font>

复制代码

定时器TIM2中断里改变占空比：
9 A' j" s7 }) Y, ]) Y1 _

1. <font face="微软雅黑" size="3">
   
2. void TIM2_IRQHandler(void)
   " d- I. g! A, A8 @
3. {  
   $ A6 y: N+ ]; I# P& J
4.   if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
   4 o1 a* i, Z& o8 C( A
5.   {
   
6.     if(motor_flag==1)//反转
   
7.     {
   
8.       TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较，来设置占空比,引脚：PC6
   % U# E) K( i4 A6 p: Y8 r
9.       TIM_SetCompare2(TIM8,0);
   3 C3 b+ E3 `* e$ H& e" X
10.     }
    7 t  g5 O+ p, k/ p+ X" I+ g
11.     if(motor_flag==0)//正转
    3 Y( Q! p% K4 |! X9 k
12.     {
    & _% {; ?5 {6 H- Q
13.       TIM_SetCompare1(TIM8,0);
    
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.   }
    . R0 Y, {3 a" z
18.   TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志位
    
19. }
    
20. </font>

复制代码

" q3 {9 v) @' ]' c: H

优秀