概述# M& i% ^) ^* {; g
小车外形:+ X6 o0 D/ k0 o" d8 d. W# B& x
# h& E1 F: T# ]# }& o5 H' s# B
" J) T/ ?4 r4 N
# L C, {# ?* C3 U7 k0 m, p( g功能简介0 {' I( A2 g4 D6 o4 B4 ?" e
利用摄像头识别前车尾部的AprilTag,得到前车位置,传回stm32主控板处理,使两车在行驶时保持恒定距离,实现自动跟车。
& p( z& I0 p9 P1 b9 ~3 L5 P8 d! g& j# h, w) ]: P5 B4 a
7 @0 t e& V; \5 @7 g4 [; h- b& m q: B" N/ l: |
4 ^0 C2 R, w% ~& b1 AopenMV4摄像头, o( _ D9 T% [& t% M9 i
1.1 Apriltag识别与串口传输2 w9 h! k) ?& |( C3 e
# E& V$ l7 ^& y9 [9 Q/ Q
3 m! O7 m2 h0 I; h. AAprilTag是一个视觉基准库,在AR,机器人,相机校准领域广泛使用。通过特定的标志(与二维码相似,但是降低了复杂度以满足实时性要求),可以快速地检测标志,并计算相对位置。
6 N$ @6 I7 b2 A7 w2 N0 V1 M% `% e( {$ t" e) q5 ]! ^
8 W' x2 |* ^$ h
Apriltag示例:/ M$ X! U& ]& u: L
% r) Z. N) f: C5 z* i5 d- t9 b
通过识别Apriltag,可以得到x,y,z三个方向的距离以及偏移角度。这里只需要三维的距离即可,通过串口传回stm32.' C+ T& P+ p ?4 g$ {6 ~1 q. E
- <font face="微软雅黑" size="3">5 a$ x9 ^' d6 C" N2 P5 P
- import sensor, image, time, math,pyb% C$ g" t: o+ [1 a, f
- from pyb import UART0 S2 a3 p; K/ K% o: u7 t
' @; `0 [ C/ A4 t; r# r- sensor.reset()
! h" X3 e" j& D7 F3 ` - sensor.set_pixformat(sensor.RGB565)0 c: y7 d9 X1 J, l; l
- sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
- a. \# f D3 Y. M" P/ Y {4 G - sensor.skip_frames(time = 2000)
" \2 u) o- v2 _ P7 P0 y) ]1 j - sensor.set_auto_gain(False) # must turn this off to prevent image washout...
8 m( i- e( V R, S% a6 H; D; w! G - sensor.set_auto_whitebal(False) # must turn this off to prevent image washout...6 D7 s0 D% V8 u' P
- clock = time.clock()
. U! L0 p4 m/ ]1 Q# E: R" H - uart = UART(3, 115200)#串口波特率
: G7 G/ u8 I, P: K, m
0 h q( r, v: _- f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
/ v" H# M" p, T8 V. H - f_y = (2.8 / 2.952) * 120 # find_apriltags defaults to this if not set
6 n" a8 {7 j2 m" h) h; }, F - c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)) N8 r* V4 w6 M
- c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5); z& `2 J4 ?6 M" a. ^
# s5 b. a5 _; O t7 m7 H- k- def degrees(radians): m# u& U) Y4 G1 N6 D
- return (180 * radians) / math.pi* a. X& q- s! {
& n2 I2 d Q' J, l8 `- while(True):& l4 T5 e; x: ]. p% ?
- clock.tick()
6 {) ^. [3 E# |( n6 `" t - img = sensor.snapshot()7 }5 u. ~6 @" l) j; G: V
- for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H113 S G# V& @# i) g( X% I- \: D. o
- img.draw_rectangle(tag.rect(), color = (255, 0, 0))* F6 L$ g, o0 a% ^; l5 h ?8 L6 D6 }
- img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
) k. I& `9 o0 R' t9 h T1 C7 x$ w - print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
7 `0 `! s# J9 `& { - #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
: C2 S% e1 ]- a# [1 [ - # Translation units are unknown. Rotation units are in degrees.- o: {% t! O l* k1 i7 G, P: B
- # print("Tx %f, Ty %f, Tz %f" % print_args)2 @1 F. i" g' L
- uart.write("A%.2f,B%.2f,C%.2f," % print_args+'\r\n')#设置特定格式,以便于stm32分割取得数据
G- H* C* [' F* n* L - #pyb.delay(500)8 l2 V- W2 X' K0 Z1 Q
- # print(clock.fps())</font>
复制代码 STM32主控板(型号为F407)
& {0 B; f6 N$ f4 S6 a3 O+ ^
- B! U/ {1 H! a, W4 g6 G/ z U6 W
2.1 时钟与中断配置$ h" A' `8 @, o
+ M2 A( s6 l) t2 Y, ^
" O6 R; X) [2 ~: Q2 L' k( W5 w* N附上stm32时钟示意图:
: T4 |! I+ H! v1 [9 S; _2 B* h
2 _' L& O" E* q$ t3 I8 h6 C定时器示意图:) R4 T3 g- \. u5 I
/ U, T7 F! I3 E% @# A1 ~6 z7 {4 i
定时器分配:
B: ~+ k7 q$ @6 r- F, j: `, [
0 u) U7 ]5 d* f$ v
所有时钟初始化的函数:(每个函数的详细内容在后面)
' q7 m; e' Z0 D5 e% l. D4 @7 M' p- <font face="微软雅黑" size="3">" t9 m8 G1 g: H
- TIM8_PWM_Init(400-1,20-1); //用于控制电机,168M/20=8.4Mhz的计数频率,重装载值400,所以PWM频率为 8.4M/400=21Khz.
0 N& Z8 e ?/ }& C$ G - TIM3_PWM_Init(200-1,8400-1);//用于控制舵机,50HZ2 J* Z6 h9 s% Z/ K Z
- TIM2_Int_Init(400-1,20-1);//定时中断,21KHZ
5 r$ \+ |6 h' g. n - TIM7_Int_Init(500-1,8400-1);//用于编码器计数,20HZ,50ms中断一次
% v7 f( p* o A6 v0 P8 e - uart_init(115200); //初始化串口1波特率为115200, U! J# s. A4 Z* b1 w6 p
- Encoder_Init_TIM4();//编码器接口初始化</font>
复制代码 2.2 串口收发与数据处理; b _4 O, j+ w! z
9 v- w* T9 l# _1 K0 J* F* c1 p+ k" H2 C8 {4 o
串口中断:USART1,USART2
6 O# {% b7 O) k0 x' a& M串口初始化函数(以USART1为例):
; p: Z2 \/ A4 S$ ^- <font face="微软雅黑" size="3">2 p; w4 B: @. {) D. S# e7 E8 E
- void uart_init(u32 bound){5 a4 {, B6 `. d1 O$ C3 Y
- //GPIO端口设置; F& V: z p d( y) {
- GPIO_InitTypeDef GPIO_InitStructure;7 ^5 v9 `! Z( A& T& y
- USART_InitTypeDef USART_InitStructure;$ C Z* O/ d9 S) Y, O* J
- NVIC_InitTypeDef NVIC_InitStructure;
1 O0 e1 O7 v u7 T3 \+ D - 4 H- C, X# @6 ~* c* w& d! U
- RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟( `( ?* e! E, l3 P+ n- [
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1时钟* v/ A# I5 I$ e
-
V" D$ F2 {: I- s7 E" d - //串口1对应引脚复用映射
: P0 |1 E7 g/ y - GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9复用为USART1# F8 O, q; H: a1 _4 M% ]+ T
- GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10复用为USART1
7 i( s( g4 i4 J -
" P8 h% x5 b2 m$ a - //USART1端口配置6 w) b6 T" i% @- u# j% S% K
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9与GPIOA10
5 P* B0 Q8 f n5 I% u0 c1 @ - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
( Q. ]; Z# i$ `' I - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度50MHz. M: e: G: l7 {0 \; Q: x2 \* E8 H# z* Y
- GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出. X x! q6 l6 {' P4 Q- P$ z8 Q R3 K
- GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
4 a7 j" l) {# ~+ D, L - GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9,PA10
6 t" E+ F* o( X+ X) @8 O0 b6 c# l Q
( ?' W* S$ O! `# ~) M; I0 t* \ D- //USART1 初始化设置, a! Z5 c4 Q: V; v6 }2 G3 r* [" r
- USART_InitStructure.USART_BaudRate = bound;//波特率设置( T5 s9 O7 O8 o. y
- USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式" i( z: A6 B7 B, P
- USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位( u/ s; P' F: ~
- USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
5 n, T4 l( Y5 x/ X5 F* K4 `6 f - USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制! D! j8 Z% W7 S0 y- U
- USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
3 K% H% W- X5 `0 P" w; u f - USART_Init(USART1, &USART_InitStructure); //初始化串口1' m/ [+ ^6 H) ^1 E& \
- , ^1 d- n8 B& W" h2 |& t6 K G- r
- USART_Cmd(USART1, ENABLE); //使能串口1
5 I, [9 f3 {" ^& ^! d; ` - ! Y# I& c" x- O) o% n
- USART_ClearFlag(USART1, USART_FLAG_TC);
+ u2 E9 a5 m) c5 _) C - 7 F7 y: R- Q. \' e& i
- #if EN_USART1_RX
' m8 O' S% S [& h$ H, Y - USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启相关中断9 e# W8 w: U& v1 [# `) C4 h
9 P/ p) x J# U" P2 L) n- //Usart1 NVIC 配置* `+ U) F' z1 J) x
- NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中断通道) w' ?2 h! j) {
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
8 O, U, f, w$ f6 ] - NVIC_InitStructure.NVIC_IRQChannelSubPriority =3; //子优先级30 Z9 v! @& g: q( f6 }& Y
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
9 p- V' O0 a" ]1 Z/ l/ s - NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器、, H \6 w2 T! ?, B5 r0 [
- 7 {0 b5 G! k; y5 u5 M/ P
- #endif; V8 }* Z1 t: Y2 q: Q2 J1 o4 {
-
, L+ |! j) K5 E e8 x6 w/ f! M - }</font>
复制代码 串口中断处理函数:; G# Y! e3 B0 @- z5 @; t
- <font face="微软雅黑" size="3">+ l; V: c) X0 m! @. _
- void USART1_IRQHandler(void) //串口1中断服务程序6 m" l- t* [' U% G3 R
- {
5 j* i' `0 p; c3 n - u8 Res;9 a* M/ r3 q7 r+ a' m6 j# N
- #ifdef OS_TICKS_PER_SEC //如果时钟节拍数定义了,说明要使用ucosII了.% n" {. L- l1 x0 Q E
- OSIntEnter(); 8 n( z7 o3 [7 ?
- #endif+ l: e" j- M: i; x$ E) X& T: j
- if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收中断(接收到的数据必须是0x0d 0x0a结尾)
( O$ ?/ q5 U0 Z* } y8 `2 O, i - {
1 \: O# M g; V4 q3 j* u$ d - Res =USART_ReceiveData(USART1);//(USART1->DR); //读取接收到的数据( o- M! K4 t, x) L3 _
-
3 x! b! N) a9 r3 H1 Z& i# c" W& V - if((USART_RX_STA&0x8000)==0)//接收未完成
3 s r t' z# z1 v - {, |6 N5 m: o) ?- l$ N
- if(USART_RX_STA&0x4000)//接收到了0x0d
7 W$ \" Y8 ^ z! s - {: Y F! a% X: h4 U, ~ X
- if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始
/ b+ D3 _& ?: D, |4 V - else USART_RX_STA|=0x8000; //接收完成了 3 y4 i& ~7 j; U! H
- }) V; I8 K7 I. S) U4 f
- else //还没收到0X0D+ J; Q- S$ L8 Z- [8 U9 o
- {
: i8 }7 O9 d$ @5 q; Q `# o* ] - if(Res==0x0d)USART_RX_STA|=0x4000;
p$ T' V; f: H' g! `. H - else
' l& H6 }* Y6 G# o0 c" h+ \ - {
. D2 s4 J) q$ D: { - USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ;8 c8 ~" u& S" q- \- s# _
- USART_RX_STA++;
9 S0 k( ]) p; N - if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收数据错误,重新开始接收
0 B/ t& d8 q* ~ - }
2 f. c) J" x: V5 K+ P9 F! x - }
% D- ?, B0 g; P% e: w& ]' b7 L - }
, w/ ? f1 p8 @9 U4 x5 H - }
6 ]' d4 E! a, N0 v; G - #ifdef OS_TICKS_PER_SEC //如果时钟节拍数定义了,说明要使用ucosII了.+ H$ o$ i! c) d! b
- OSIntExit();
1 D8 E i+ @' ]4 X. g9 K: U - #endif
! w* }; y" u* m- g/ ~ - } $ ]% A( K& R" Y% h r2 m, w, q, x
- #endif </font>
复制代码 字符串接收与处理(从openMV接收到的数据):+ g8 V5 w0 t- v9 X; X0 g
- <font face="微软雅黑" size="3">/*涉及到的全局变量
& [( p5 H' y2 \8 u" v. a - float data[3];//x,y,z方向的距离,浮点数形式9 K2 R, S/ O1 i& }. ?
- unsigned char data_string[3][7];//x,y,z方向的距离,字符串形式 M/ C$ P5 X! N" R
- */
8 j+ r; E6 Y" U# S/ c3 d# l- c - if(USART_RX_STA&0x8000)8 q* j) G" `- b @ K* D
- { & l- H" u5 ?: M. [' }5 ^. h( Y# w
- //清空字符串' k4 R! v2 [8 ^7 V* Z
- for(i=0;i<2;i++)
- n) f8 K1 G! J) p% U. y% P/ k0 D - {
6 n& w F3 k" W B( H. @8 l3 U - for(j=0;j<6;j++)9 K' W, q# w# m4 G
- {
8 M2 h5 ^& h# d6 w# @( [" u - data_string[i][j]=' ';
0 }5 L) Z8 ?# {& L S - }/ W- e2 p: Y2 L1 s3 R
- }8 Z: y! R8 X6 _
- len=USART_RX_STA&0x3fff;//得到此次接收到的数据长度; V; R, J+ e7 V
- for(t=0,j=0;t<len;t++)6 ` J$ F8 ]& R# o+ q- X) }% _7 V+ ?
- {5 S' ]1 E3 F. @( j& J# S0 z
- j=0;
$ m7 Z6 o% }9 n2 F$ z* C$ U - if(USART_RX_BUF[t]=='A')
! e0 x5 @5 P: a2 \3 D! m - {& r4 `. o; T. ?& S1 [& B1 V, r
- t++;//去掉首字母# j! m( c T2 K3 h4 | d+ W7 O
- while(USART_RX_BUF[t]!=',')
4 z( P0 Q) J, Q/ Y' h - {
? ? I- e3 e! T) Y - data_string[0][j]=USART_RX_BUF[t];" F; Z( W' Q5 M( `
- t++;" J6 C( H5 T* Y; S$ ^) m
- j++;
4 N: Z7 N. }- ~' U! e! N- R - }& C0 d5 l& l( i; W. Q* ]# }2 v
- }
0 k! b) G* q# n+ F+ Q4 @7 w3 I - if(USART_RX_BUF[t]=='B')
8 C( Z+ W- h4 K: v( Q - {7 k, L' Y* F+ c) M2 c' L7 k
- t++;//去掉首字母
* W+ M% m. S( B' _1 K - while(USART_RX_BUF[t]!=',')
9 D$ H& p- s. B/ I' V1 O% V3 w9 b - {; w. ?1 b$ a- {( B% c5 l& d" @
- data_string[1][j]=USART_RX_BUF[t];. _# P5 ?# S2 S4 n; E/ u" \5 _
- t++;
1 Z8 R( R& J- C8 N' f - j++;% J+ Y8 m& x+ W1 N, B' ] p
- }
5 }6 ], S- }' a. |/ G- A - }7 |/ N+ M# N. p' d9 H
- if(USART_RX_BUF[t]=='C')
; o& o' _6 J0 ^! _1 u* N0 y - {
: l7 K8 B0 Q9 ~; `/ J - t++;//去掉首字母
2 `/ J! s% N( W4 K - while(USART_RX_BUF[t]!=',')
4 u/ O; r7 O7 N9 k8 Q6 C' @ - {
# t& k1 e( f4 i, w {; y" J+ p4 B5 K - data_string[2][j]=USART_RX_BUF[t];! R6 ~) q4 b2 S1 a8 w2 R
- t++;
3 K, k+ V: @% ` - j++;# X1 D* Q5 x4 n! X+ Q, Y C
- }- T' P% C- L4 g! N
- }
( f' \9 L/ W) n/ d - }
' I! @! B _1 H4 q! [% |) ]5 Z C - //把字符串转化为浮点数
& j' p0 _ F+ c - data[0]=myatof(data_string[0])/100.0;//x j0 N% l) ]" a3 H
- data[1]=myatof(data_string[1])/100.0;//y: a4 E5 N& S5 c6 X( K5 ]1 d4 u
- data[2]=myatof(data_string[2])*(-1)/100.0;//z,输入是负数,转换为正
$ _% W5 O4 ^* J, [& Y0 H" ^ - //USART2发送数据
; W9 k; T/ a) K9 H% G8 W9 s - // Usart_SendString( RS232_USART, (uint8_t *)USART_RX_BUF );
- Q0 `* g, n8 ^5 \. N7 Z2 J -
4 L) {+ @8 {: D' o! L3 d) l - //LCD更新显示
3 Z8 ?" b4 i2 @* l. b) u0 R - //显示xyz
! b$ Q& [8 A) V3 ` - // CLEAR(10);
' |( }# L; b6 s2 K" y - // Gui_DrawFont_GBK16(30,0,BLACK,WHITE,clear);8 f7 c. W3 A9 [* {# H. H
- // Gui_DrawFont_GBK16(30,0,BLACK,WHITE,data_string[0]);' b% J: c1 n9 j" R+ K5 o( T; ^
- // Gui_DrawFont_GBK16(30,20,BLACK,WHITE,clear);
$ V1 N4 Z$ m6 W4 X - // Gui_DrawFont_GBK16(30,20,BLACK,WHITE,data_string[1]);
& g) j+ N) G8 ?+ `5 Y - // Gui_DrawFont_GBK16(30,40,BLACK,WHITE,clear);
8 }# B, Y1 X7 J0 T; s: ]) I( D - // Gui_DrawFont_GBK16(30,40,BLACK,WHITE,data_string[2]);# F ]1 H8 ^* ?0 V7 t) P
- $ y7 ^& U& s* u b6 T ^
- USART_RX_STA=0;//清除标志位, e9 {. q' M; g3 s* c
- }0 M0 [& o( D0 S) R
- }
. N |3 U M& E- C" N6 J - </font>
复制代码 字符串转化为两位小数浮点数(用于后续PID控制):$ g7 E# |% b- w% U
9 X2 Z5 x# L. C8 Y, V9 S. Q- <font face="微软雅黑" size="3">int myatof(const char *str)//此函数仅适用于两位小数的浮点数,返回的是乘100后的int值,因float返回有错误
8 @) [$ V1 b; ?% t) Q3 y - {
; i% e1 z' `! O - int flag = 1;//表示正数2 F7 _! C% l! ]+ [
- int res =0;
/ y# N# x" D$ G! F3 h, A3 P3 W - u8 i=1; //小数点后两位4 h9 C/ `9 {6 o5 [# v, z: C* N
- while(*str != '\0')
: h! Z+ @* z/ c$ T; w3 l( L% V. y - {
9 t, L% z6 F0 P: |: v/ P6 n0 c - if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一个数字5 a. G+ M- @, j2 E( p. z' n$ M6 B& k
- {8 _" G: s8 g! Z( c- I# s
- str++;$ g, ?+ i% A. p
- continue;, u/ Q( Z) N* ?- n* q: D
- }
* D( t, M/ L4 S4 P( h - if(*(str-1) == '-')0 @% [ F% s& z1 D# u
- {
+ ]4 M. E3 N5 E- U2 g) u6 w4 D4 j - flag=-1;//表示是一个负数
+ |3 q" m% w' [+ M& u& c - }
- I" V$ a [1 _1 U) w1 E
5 Q v6 D5 a' X) l3 b# G* W- while(*str >= '0' && *str <= '9'). s Z$ J9 m% @3 A
- {
) K1 z1 l1 `$ D- ?9 b( F9 Y - res = res *10 + (*str - '0');8 r' a: t) y6 [) @! ~
- str++;
' i$ h/ u) Z( a" q3 H: D - }
, X3 T. e+ \8 h9 y8 a4 n# N - if(*str == '.'), n# c% d, `; t6 y7 a
- {: t' K& z5 [- v [# u! o) t
- str++;
. ?: ]! W* Y) H - res = res *10 + (*str - '0');# {0 u3 t9 Q; E2 |+ K% b x4 v0 m
- str++;
7 D9 c, b p1 T! L. i/ G; ] - res = res *10 + (*str - '0');//保留两位,故加两次
; \" G+ Q# g. o0 [$ Z( v6 M - return res*flag;
0 O2 v3 b0 p+ G ^, H- e - }9 P3 F/ m) v" x" X9 m1 C. g# w5 i
- }
; }3 G' b6 L* _4 `" J - }+ ]2 W6 e7 {. E( K& u& B
- % D z8 ?( x: @7 p& B
- 8 K1 m0 p4 D8 ~7 \
- 2.3 LCD显示模块
$ I5 u$ ^" _6 r7 }& n! X: K
5 S+ Z; S7 D8 }* u k- LCD模块用于调试时观察数据,调试完成可以删去,因为显示屏很耗时,使处理速度变慢1 X( O, O9 C1 `: Q. t0 y( r% u
- 驱动函数总览:1 T8 w7 I- |+ d0 r
- 3 l0 Q9 U5 Z; a+ G9 N
: |# x A2 |- F. w/ f- 5 }- F, A+ q. n- g. r. Z
- void LCD_GPIO_Init(void);
4 D+ }' j2 P/ c. u5 N - void Lcd_WriteIndex(u8 Index);
9 ^9 h" _- S9 Z$ w# [. s' ` - void Lcd_WriteData(u8 Data);
+ A3 C# b, l- m& n* Z3 E - void Lcd_WriteReg(u8 Index,u8 Data);
% j; C) w/ k6 W/ t - u16 Lcd_ReadReg(u8 LCD_Reg);* ?9 ]( p, O8 h% O" o3 A
- void Lcd_Reset(void);6 H9 [4 q8 h- a& b1 t
- void Lcd_Init(void);- M! r6 g+ K! A% t0 s; H/ n
- void Lcd_Clear(u16 Color);2 l- x, e# V3 z5 c+ t2 d/ Y# Y6 B
- void Lcd_SetXY(u16 x,u16 y);
& `, a( K! h4 C7 u* L! [9 C - void Gui_DrawPoint(u16 x,u16 y,u16 Data); L8 J; f) H# [. M
- unsigned int Lcd_ReadPoint(u16 x,u16 y);
. X7 ^) x$ n$ a - void Lcd_SetRegion(u16 x_start,u16 y_start,u16 x_end,u16 y_end); m# j/ ~0 w! J: x6 M
- void LCD_WriteData_16Bit(u16 Data);</font>
复制代码
: B5 v, ^, i3 y$ Y, P5 @
( s" u* Q/ t: W/ L* S/ `TFT屏幕初始化:
& K( {5 M- e l$ b* y/ h
0 o7 c7 _+ N" T& ?( C- <font face="微软雅黑" size="3">void TFT_Init_Show(void)# `0 m& B0 B" i0 Q9 R
- {
' O! `/ C% _4 B) r0 i! E - Lcd_Clear(WHITE);
% G3 L7 W) Z* }1 H$ v - Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN");+ u" q8 R1 e) x
- delay_ms(1000);
3 K* u* p; H' `+ i5 f8 T - Lcd_Clear(WHITE);: @, u9 e# ^- }+ ?5 M5 u+ g# u
- Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:");
: R! c0 X# t/ C* U: v - Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:");" _& t" P' S/ s" o
- Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:");
% q9 E5 u2 Z3 }! }* e# K - Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");
; A3 ` d; g5 h# v( l2 V: o - }</font>
复制代码 字符串显示函数;
4 i5 R+ Z$ ~7 u% I! c" z- <font face="微软雅黑" size="3">
. h/ [+ x% u0 {! n - void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s)
, `, |7 d; a, b - {
3 ~2 m1 h1 G, m; y3 A. @ - unsigned char i,j;
/ `1 i% X# K" x( k, d - unsigned short k,x0;
% y k3 p, u8 Z: T4 H2 _6 f - x0=x;: t ^" L% s2 _0 `
- 2 _7 U+ a% ^, J# }, f
- while(*s)
$ m1 ^" a2 I8 ]* i - {
* Q: O$ \& D" {! C - if((*s) < 128) 0 P8 l0 r; \/ \; [$ T
- {9 r4 j# s2 {- H/ d8 ]
- k=*s;$ w" U4 N& A$ m
- if (k==13)
K6 y: w/ q* M. I7 w& U% u/ g - {
! H) g3 }7 y: l+ S7 @6 C - x=x0;
& [; l" ?9 { _ - y+=16;
& o, t! ^3 J- `3 b - }6 L7 a& E4 o; C0 s Y
- else - V7 C6 B/ X' b. b) Y$ h
- {
% c! M' J2 |. E9 G! g) w - if (k>32) k-=32; else k=0;" L- ~% N, b2 }2 W2 {" I& x2 J
-
/ v r& Y1 m- E - for(i=0;i<16;i++)+ h! e. l4 `$ H7 [ k. B: t
- for(j=0;j<8;j++) / j* a3 l2 \, [7 A
- {
D+ A5 F& J9 Q0 x* j - if(asc16[k*16+i]&(0x80>>j)) Gui_DrawPoint(x+j,y+i,fc);
& \0 i' N1 u& b6 g* ^ ?' Q - else
5 L' Z8 o! |+ u- v& o F9 |! k - {6 E( h: h0 }8 a' [- T
- if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);
" {$ ~$ s: R3 Q% i0 V/ O - }' H2 s8 R% g; \% O8 ?( ~( {7 Y
- }8 ?* I* j% I7 `9 u2 ?5 a
- x+=8;
# [6 @; Z; u! _. ~2 u) J - }, s1 ?6 _7 W1 m1 Z- d. P( N. O5 Q
- s++;; @1 O7 Y3 R; L0 ]' }, ^
- }& K: h0 A4 F' l/ s G1 Q
-
1 x4 L% Z' O9 q+ _" U% J/ z" U - else - D: a7 u- N2 T' ]& @
- {
1 q0 D7 H% G# R3 A% i -
+ A! o1 \+ H( [9 K, B1 w& V - 9 w W% @2 b4 A8 H0 k( n7 w
- for (k=0;k<hz16_num;k++)
' X6 r" v. Y P - {
4 [0 ~: G! R9 R2 d K& q! I - if ((hz16[k].Index[0]==*(s))&&(hz16[k].Index[1]==*(s+1)))
# N% o6 v0 O% |9 \ - {
2 o, d, g( Y2 ^8 E2 @5 i - for(i=0;i<16;i++)
) b2 U* P9 c( n q - {
7 L0 o5 K M" c! C+ C - for(j=0;j<8;j++) 2 \0 q4 C$ [- g+ f* s
- {2 A4 f; X% Y. ]+ |
- if(hz16[k].Msk[i*2]&(0x80>>j)) Gui_DrawPoint(x+j,y+i,fc);
% w8 ~/ D& p) `, l - else {
9 J2 T! i# a5 h, E0 o# t8 Z% ~% ?5 N - if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc);2 y% _) i. g, u
- }; \& H4 j! ^! }7 e* u
- }6 U9 @* b, c) x) \$ H l n
- for(j=0;j<8;j++)
6 ]# Z/ p3 k9 A" O1 v: \/ H - {/ y5 X" H- Z) k+ u5 t9 @3 ]3 E
- if(hz16[k].Msk[i*2+1]&(0x80>>j)) Gui_DrawPoint(x+j+8,y+i,fc);2 h& u! t8 m3 \! t7 @, t
- else " X' \/ K, H- x& y
- {$ D- v2 g0 L% M3 w# s
- if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc);5 z! |6 ]: }, r: E. o
- }4 a7 f: R" P% ]/ s1 v
- }$ M8 i3 K$ a( J2 b) q: E$ C6 N0 a
- }
. l3 a; m- {6 q( _ - }9 N/ f- X6 a( g, P4 Z3 {
- }
[# Z0 z1 a( i+ j: S/ j( { - s+=2;x+=16;
: |" T/ F9 i& N% k1 S - } # \. M) z) K, s
- $ w5 t7 e3 x X( Q% {
- }
3 T4 J, w( J9 P3 o3 v( ^- X - }
) Y$ I% h$ D2 R6 m* |: O - </font>
复制代码 2.4 电机、舵机与编码器
# T) i/ K2 y' U+ B
' g5 l( W3 F8 M/ M$ ~/ |' j
8 f. Z1 R- b4 |0 N定时中断:TIM2,用于修改电机和舵机的PWM占空比
; B6 ^4 [% x4 r初始化函数:
9 t+ H6 E. G1 @, o2 o+ L/ x7 b' i" f* ]" `8 B9 u$ }
- <font face="微软雅黑" size="3">
( p# w8 ?6 D& A6 m - //通用定时器2中断初始化" x8 W- `; V; R: p2 W- f
- //arr:自动重装值。
8 p; k6 K- ?! @' i7 I* } - //psc:时钟预分频数
/ e( n0 f1 q; t7 D g5 Z - //定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.# ~. i. H4 |0 w2 `8 `& U4 Y
- //Ft=定时器工作频率,单位:Mhz/ d; Z# G1 I* O
- //这里使用的是定时器2!/ T, Y1 I* L% |- S& ^
- void TIM2_Int_Init(u16 arr,u16 psc)
; V6 U- a3 W8 [; V! g4 `; E - {! w9 _* }( Z8 i5 _
- TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;! H1 m7 ^1 D6 S9 O
- NVIC_InitTypeDef NVIC_InitStructure;
4 a8 R( [. J8 p3 W% I - - z8 k) r0 D9 G! c" [
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE); ///使能TIM2时钟
M6 c' M$ @) y- n* |( ?6 P - $ d* q' ?$ i' u
- TIM_TimeBaseInitStructure.TIM_Period = arr; //自动重装载值) G% R3 J7 F' m9 E1 E
- TIM_TimeBaseInitStructure.TIM_Prescaler=psc; //定时器分频: P G' x, N) E: A3 a
- TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式, Y5 t$ y5 A6 m2 N& i2 I( q3 g
- TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; 2 b% T2 A m$ a9 j. t* s
-
( O8 u. r0 i }+ P/ J4 t - TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3 ~: b% V( X% g2 a4 e
-
7 s- L/ C% g$ n - TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允许定时器2更新中断9 @+ L% {% T E% p0 j
- TIM_Cmd(TIM2,ENABLE); //使能定时器2
; r: m; g; o7 O2 B4 _. s -
# `' J2 o2 ?5 A |# g1 ^ - NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器2中断( P9 b" o+ |. [; h8 R8 y
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //抢占优先级18 {! i6 P t. I. @6 J9 P% |
- NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子优先级3
& m$ C, l u% Z' D* ~! `$ V) t - NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;) V4 t) |6 [9 r1 ]. R- E+ M
- NVIC_Init(&NVIC_InitStructure);% w8 j9 I$ o7 h$ ?7 t) B t3 l3 d+ Q
- O- b$ s# U! N- i1 H- }</font>
复制代码 TIM2中断处理函数:4 @0 e& R$ a: R
- <font face="微软雅黑" size="3">6 t( z3 P( k. q) ]0 [0 x5 ^
- void TIM2_IRQHandler(void)
& J9 a# S d! C - { + I2 T' g: E. x3 ~- B( R- b9 X) {
- if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
l- w0 k: F5 n - {
! O5 {0 |: c4 ^" t - if(motor_flag==1)//反转8 F+ i: z0 ~; Q0 N# U9 ]3 o# y* A
- {2 _. a2 |( b7 R" E4 p) S$ E& t
- TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较,来设置占空比,引脚:PC6
& W5 I Z8 _6 d% }( W1 H - TIM_SetCompare2(TIM8,0);& o8 z0 X1 y" w @/ ?; Y- C+ f
- }' l7 y( r0 m( ~9 V ?' V) F F5 H
- if(motor_flag==0)//正转
# `% `! ?! |! K! {, g. |% M" Y - {2 d3 O7 I( n0 R
- TIM_SetCompare1(TIM8,0);* n" l( u9 b- e8 e7 D
- TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较,来设置占空比,引脚:PC7
9 M- q9 v8 R& U# q - }' p- U' o8 M! @
- TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度,引脚:PA6, V+ y- p; Z( x# F6 X, Z% }3 v
- }
, ^( U r, o ?% [' D% a - TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中断标志位. b' u; h* ~) r5 f/ D2 l
- }6 K, r/ F9 X5 f: t
- </font>
复制代码 PWM输出:TIM3(舵机),TIM8(电机)9 r. l- e, U: b
初始化函数(以TIM8为例):
3 v( d6 x+ @) p7 B$ `
/ |9 I; ] U# o" H0 G! B. q- <font face="微软雅黑" size="3">$ t; T f1 g) [8 l" [! r3 k. C
- void TIM8_PWM_Init(u32 arr,u32 psc)+ U/ e. q5 X; X; g {' h
- {
: ~& b* m& q+ V. S - //此部分需手动修改IO口设置6 s: j4 A" P1 M* B# T# D
- . K# ?- }% I/ G2 T
- GPIO_InitTypeDef GPIO_InitStructure;
* r( F9 \) A5 `1 D; Y+ l0 }% ^ - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
/ b/ m+ H) s$ Y, } - TIM_OCInitTypeDef TIM_OCInitStructure;
$ O9 ]$ Z. q# r/ S* g/ l - TIM_BDTRInitTypeDef TIM_BDTRInitStructure;
2 b. _, w+ w% T }# C8 o - ; K6 P" v4 S4 ]; H. v
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE); //TIM8时钟使能 ) N- u" Z+ m/ i; }/ N1 i
- RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE); //使能PORTA时钟 5 v9 e1 e0 ~" [1 h \4 _2 H( x
- 4 n ~( ?* n+ X- c
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7; //GPIOFA
G w1 S7 F) j) C# X M& [ - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用功能
; Z% ^" i: ]) D2 v# |) o6 o9 y - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度100MHz
8 i o% z3 b5 E: n& ?8 L - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
. F! O9 |5 e; J+ H - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
1 ` b7 O1 d# f- } - GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA7, z; i, |7 t# j4 }
; ^1 ~# W- [/ {2 ^$ }6 p' e- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
; X; r2 A8 N9 B$ {! }2 c - GPIO_Init(GPIOB,&GPIO_InitStructure);7 `+ ]' @# S) i8 V; m {' S2 t
-
# ^0 K5 ~6 u% _# Y2 I+ }# W - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;
4 g1 n7 D ~- z2 @ - GPIO_Init(GPIOC,&GPIO_InitStructure);) E6 ?* L \" r5 O/ G4 h/ n* n
-
; q6 {- I5 Z4 Y) Z& d2 l - GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8复用为定时器1
1 l7 ]9 `' M' S$ I! I% ?8 { - GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9复用为定时器1+ m% V) z* x1 N6 z9 i
- GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10复用为定时器1
6 ?( K0 s2 E' a9 { - GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13复用为定时器1
( \) u. A& b i* w7 N - GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14复用为定时器1! O! m' u0 }" b' Q7 @$ O+ X
- GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15复用为定时器1" S1 e# y- A5 u
- " \* l t; H- p' k& L
- TIM_TimeBaseStructure.TIM_Prescaler=psc; //定时器分频
5 v& d; h0 C% E* s8 Z7 y5 P - TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式' G; T v" n* ^. E! [! u8 w
- TIM_TimeBaseStructure.TIM_Period=arr; //自动重装载值
- _# ^9 C& h) V) d6 Q: ~ - TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1; + k' Y8 Q* p0 k
- # E' X1 A/ `" C* l
- TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定时器1# X; W4 D& w3 Z( ?* L
-
3 D$ j' ~, b d9 D4 z# ?, ^6 P - //初始化PWM模式
& a x5 o# s# [$ d+ w) @* G - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;" D. S) J0 D4 f( n
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;7 L; N! B2 J' \ O1 b
- TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;. e# Z- U5 S% D8 F. y. b- m
- TIM_OCInitStructure.TIM_Pulse = 0; t# K: b% [1 L2 U
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
6 R2 e. D7 w* m7 O. E0 Q - TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
1 K% S: R0 N7 l3 V } - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; ' |# v6 F7 q) {" t7 X/ r+ _1 E
- TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
+ b& i7 w \6 l
8 ]# U' }$ e: i6 _- TIM_OC1Init(TIM8, &TIM_OCInitStructure);
( N5 c1 Y+ e& Z - TIM_OC2Init(TIM8, &TIM_OCInitStructure);
7 G' @0 H$ N3 o6 v c - TIM_OC3Init(TIM8, &TIM_OCInitStructure);
6 ?. e; f1 }3 [- g# ]
$ H6 Z. }( y6 Z% M; p- a+ Z- TIM_OC1PreloadConfig(TIM8,TIM_OCPreload_Enable);$ @* q# L7 B* \6 N8 O
- TIM_OC2PreloadConfig(TIM8,TIM_OCPreload_Enable);5 u3 g$ R) F+ J
- TIM_OC3PreloadConfig(TIM8,TIM_OCPreload_Enable);* Z7 X2 v9 Y. V
- # H" m I7 A) E1 f2 }: \7 e
- TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable; @" D5 \! w [3 v, y$ P8 P, u
- TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
3 s, x4 I9 a! ~: g* V - TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
' z# n% i2 d+ `5 T% q3 I! S- J - TIM_BDTRInitStructure.TIM_DeadTime = 0;
- v) ~' h& j* c) X - TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
6 R( S' o' W" \0 l- L6 h# ? - TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low;* {- {4 r" }, D' h
- TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
; K l$ T/ \( [1 O# ^) N1 p( k - TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure);7 U } ]# G) h, E+ z
- , h+ P9 B& h( T5 C
- TIM_Cmd(TIM8,ENABLE);' z1 U$ O; v. B, Q5 j
- TIM_CCPreloadControl(TIM8,ENABLE);
% F0 n* H0 S1 I! N. ~ - TIM_CtrlPWMOutputs(TIM8,ENABLE);
0 W* M) d. U- ]& z: w Y' o - $ X8 A( A% k. s7 z" |6 g2 p
- }</font>
复制代码 编码器初始化函数:( ~& P7 A7 e6 L' c* ]0 h% A3 l
- <font face="微软雅黑" size="3">
9 r+ c% r$ R# T I( a - void Encoder_Init_TIM4(void)
, I$ x# L' o* m/ O" G - {/ @/ Y9 m1 j$ c
- GPIO_InitTypeDef GPIO_InitStructure; 1 g6 }# ~& d6 W/ W8 n
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
' [) G& P8 n4 G - TIM_ICInitTypeDef TIM_ICInitStructure;
* s4 t& p8 p" i+ I" }7 N2 y) ?' N; F9 S - NVIC_InitTypeDef NVIC_InitStructure;, y K: @6 q! i1 F: o
-
' { S. F7 _, {' H `! F3 B - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//开启TIM4时钟
0 f8 I! F/ t, U( I. L - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//开启GPIOB时钟/ x* J8 a3 h. w& b' ]. }3 ^. g
-
5 F) J% J( \0 P& R8 J" D - GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引脚复用3 T s+ b# k8 B5 U! Z0 _7 t6 J, Z M
- GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引脚服用. V0 Z* g0 \8 n) O' n
-
! x0 \- C6 V0 A/ j$ F - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7
4 ~+ b& v. U/ |$ y } - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;- h% P {7 a1 ]* h
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;1 a9 a, ? [" ^# f
- GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;4 b8 S. S; [4 F7 E
- //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ;
1 X A' T/ y% \( y' }* {5 ~ - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;2 s9 K9 s$ L+ e
- GPIO_Init(GPIOB,&GPIO_InitStructure);
& x R& @; o3 _% `; t% I3 t -
% y7 t n+ i6 u( W" S - NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
" Q: d' P- T" H0 F - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的抢占优先级可以尽量设置低点*/# e8 R. v2 d6 W2 y
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
" b K" U1 J6 W* O! c - NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中断,防止计数溢出而没有相应函数,造成卡死' S4 _1 [8 u' V5 Z5 K2 b# z y
- NVIC_Init(&NVIC_InitStructure);, E' x! N H/ X# H4 F2 A
-
1 ~4 T) t d. E! @0 J4 P" y7 V6 O& `5 G -
& c2 T4 S/ V# ?9 j8 ? b0 |* k" Y" c - , c( M( [1 P/ N1 h z& n
- TIM_TimeBaseStructure.TIM_Period = 4095; //设置下一个更新事件装入活动的自动重装载寄存器周期的值
5 M8 V/ u0 k. j' F- L - TIM_TimeBaseStructure.TIM_Prescaler = 0; //设置用来作为TIMx时钟频率除数的预分频值 不分频/ g y( [* q7 e+ ?1 i
- TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
6 l- t9 @7 |! v: G0 }) t - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式+ B$ B* b* P5 Z: Z
- TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); ) g$ ]6 |3 d: e# y* c0 j
-
$ d3 f1 P3 X: [+ X - TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling);
- i2 q Z. `6 T' D, d - TIM_ICStructInit(&TIM_ICInitStructure);3 `7 x* A1 B: X6 b: t5 |' Z$ m
- TIM_ICInitStructure.TIM_ICFilter = 10; //输入滤波器" f v9 c5 D. ^6 w' d% O
- TIM_ICInit(TIM4, &TIM_ICInitStructure);* r1 N: p7 F/ l7 u% b% E% x& X9 G
- TIM_ClearFlag(TIM4, TIM_FLAG_Update); //清除所有标志位
$ Q* O9 H4 {3 B$ W+ Z - TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允许中断更新
+ _8 {5 b# K" c7 g& r - TIM4->CNT = 0;
* {8 @6 Z, U: e; C% ~ - TIM_Cmd(TIM4, ENABLE); //使能TIM4
3 @+ R! Z* e' |* V9 w, C - }1 w4 ?5 H0 G; ^: H3 S5 a) E
- </font>
复制代码 编码器返回速度值:
, T9 D& S" h. _3 Z4 r$ L* ^) o& T# x5 y- <font face="微软雅黑" size="3">/**************************************************************************
7 S. ~% R9 R/ g/ L - 函数功能:单位时间读取编码器计数* @7 t6 D7 ?% }3 W
- 入口参数:定时器
9 l4 I9 L, H' H2 [1 | - 返回 值:速度值
6 w" N) b f, l( g0 P4 E; j - **************************************************************************/ w- Y$ X& [; o+ x$ g$ ^+ w
- float Read_Encoder_Speed(uint8_t TIMX)
2 t5 w; S! D5 a3 A/ D! V - {& T. c5 Q+ O5 j3 g' l$ t- J( _ L& Z
- int32_t Encoder_TIM;
6 |% U5 P9 r8 b# u - float res = 0;
" e$ ~% K- d; J3 x v - switch (TIMX)8 r/ k* N0 @5 Q/ H% x, n$ N$ a8 D
- {( K) i: ]; o8 F3 ^4 J! A
- case 5:
* B1 _7 `7 P$ I# X - Encoder_TIM = TIM_GetCounter(TIM5);
* j* C/ Z! J5 e5 m0 G - TIM5->CNT = ENCODER_BASE_COUNT; 8 K' S; f8 @3 D# s$ Q: V( k& w+ @5 M; w
- res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;
7 g, {# P3 o% T$ w9 z& b# d1 u - break;
7 U# ^% q; b; |$ m) x, S - case 4:+ S) |8 s% @1 i ]' L3 u
- Encoder_TIM = TIM_GetCounter(TIM4);
. q' j. q1 z) Y - TIM4->CNT = ENCODER_BASE_COUNT;
( n# j6 K2 S; ] - res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT;7 h. R6 `+ f/ I; p( K3 b& o
- break;; {5 o' Z! b/ h1 N. ]
- default:+ W R7 {" }! B9 c4 o1 W! ~
- Encoder_TIM = 0;7 O$ X6 G2 b1 f! y" }) V$ L: n
- res = 0;
( I: ~) c6 t1 d$ c - }* W; n# M( v: T+ z2 c6 w
- if(res>2048.0f)
+ F5 E8 g/ \; q9 \& c8 l; i - res-=4096.0f;
& T; |. S9 w5 W - return res*360.0f/4096.0f;6 j+ Q& Y. I5 w$ D( Y2 C$ d0 ~! \
- }
* Y& M+ V6 X1 D8 V' d: A8 Z) ^ - </font>
复制代码 定时从编码器取数,注意,时间不一样,取回的数值也不一样,取决于实际速度以及编码器线数。这里50ms取一次:( O5 ]( W+ B% W: V# ~) ]
- <font face="微软雅黑" size="3"> q- h! [1 m9 I g0 \( S& j g
- void TIM7_IRQHandler(void)//频率20HZ,用于编码器计数! t7 Y: g8 @' }* ^
- { 2 t4 L$ k, p+ J3 h2 @
- if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中断
: A% o" b) W3 o0 Z6 z- K - {
5 s/ O0 u# l9 r7 O6 C - speed=Read_Encoder_Speed(4);4 H% ]( J9 U+ N& l6 b* m
- }
. }" `, L; Y: S5 o - TIM_ClearITPendingBit(TIM7,TIM_IT_Update); //清除中断标志位
! E0 @4 c2 |1 Y7 q" r) w - }</font>
复制代码 2.5 PID控制( Q" e9 e C* p; j2 x" r) p' U
1 L) Y4 z% M% A# |& t, K
! Y% r M6 u; t) T6 x0 x
PID库函数:' P; W+ c# ^3 ?* u: Y0 y) M
& o5 S, @1 C* b3 ^- <font face="微软雅黑" size="3">9 @- t' x7 E. t
- #define N 2 //需要对多少变量进行pid调节
: F: O' `+ Z7 V* x1 A6 X - + W# `& k8 W! _* o y' ^- l% F
- const float KP[N]={1.3,1.0};//这里只用了比例调节
5 a" x3 X1 m% \7 Q7 Z* D I, R: v% w - const float KI[N]={0,0};
3 Q2 i+ m, T( [) n: V3 U - const float KD[N]={0,0};
& x) o( G9 o+ R
6 o) q1 L% c! U0 z N$ U4 ~5 P+ M
" T% D3 P9 C" J$ G% p+ V- struct _pid{4 J: Q% e; }8 I5 L! V
- float SetVol; //定义设定值; f5 w3 P: j) N" x" O
- float ActVol; //定义实际值
% h& B* U5 Y3 a - float Err; //定义误差" L+ H5 O, ~7 [: l# m L+ L; L5 I
- float Err_Next; //定义上一个误差# d& H! a; R; w1 j) g
- float Err_Last; //定义上上一个误差
. X5 L7 B+ W5 T& g" K& m - float Kp,Ki,Kd; //定义比例、积分、微分系数
( z) q3 V, A% w5 B9 b# E - float integral; //定义积分值( r& Z" i7 e$ o# f) W7 Y
- float actuator; //定义控制器执行变量' Q9 o8 T" A. O! E; j5 z
- }pid[N];
7 G% w9 i2 ]0 J( g: q
3 Y* m+ ]5 Y: k8 n2 i g7 f, I- void PID_Init(void)
- d0 R/ c( W+ A4 J3 E' o5 H. L - {1 x* ~1 e2 u6 ]. b5 ~9 C% U: l
- for(int i=0;i<N;i++)
5 }1 X* G1 _% p( J - {. L1 d& w, i5 ^5 ^& [
- pid[i].SetVol=0.0;
3 Y& e' @! |+ i" s3 L/ m8 y1 c - pid[i].ActVol=0.0;6 J2 Q b5 c2 q5 {* e8 q
- pid[i].Err=0.0;% Y0 h1 y2 ?! I; y% H
- pid[i].Err_Next=0.0;) s6 y [$ r# u. s1 U9 D
- pid[i].Err_Last=0.0;
+ e$ w; b) U: G; v; f - pid[i].integral=0.0;1 D( v& T; n4 ?
- pid[i].actuator=0.0;
@0 A& z+ l0 ]# E# d- u* { - pid[i].Kp=KP[i];
2 g8 T6 s: S. ?' e, Z* F1 k - pid[i].Ki=KI[i];
) [; T/ D$ J. m* e - pid[i].Kd=KD[i];7 [, a" L7 [6 G
- }" M3 M" V5 c: V/ J
- }
. N8 c. N# T$ `, q3 e7 V
2 f) X/ w) q# ^7 o z N" F- float PID_realize(float set_val,float get_val,int i) //位置型PID算法实现
+ c$ y' x: j7 z- ] - {
7 F/ w: I) K; f6 f - pid[i].SetVol=set_val;
+ B! W9 u, F7 c' f - pid[i].ActVol=get_val;
! @! N& C6 ~% K( Q5 t3 e5 n! [ - pid[i].Err=pid[i].SetVol-pid[i].ActVol;* }( U+ w. @5 ~
- float IncVol; //定义增量 d+ C7 w, C: Y6 g1 k+ l
- pid[i].integral+=pid[i].Err;
: @6 Z; ^ x* s: T3 Z3 P6 `; T - // 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);5 M# }! g: I" ^2 ~+ A" p
- 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);8 r. a4 G+ l" {+ w! u. i
- // pid[i].actuator=adc_val+IncVol;1 ?7 Y6 [2 Z9 S# V' ` x' G
- pid[i].ActVol=pid[i].actuator;
& m9 l/ q2 V; `& P7 l$ X+ ? - pid[i].Err_Last=pid[i].Err_Next;, |5 m, _ {1 U* K% S
- pid[i].Err_Next=pid[i].Err;
4 P% H! S" M4 {* a4 ^8 u -
( C, ^- X$ Z. j( E6 `+ V# [: H% X. [ - return pid[i].actuator;
: I6 _6 h. P/ W* C0 L- a - }</font>
复制代码 主函数中的PID调节:
3 k7 Z4 j) L: v" L. r% f6 h6 h; i" ^4 ]7 x/ h4 n
- <font face="微软雅黑" size="3">) C0 E6 V8 J j0 b# H/ L v
- z_get=data[2];' N" w7 O$ b4 b: d" H0 j
- x_get=data[0];- G, Y* L3 e% j( E6 y7 o
- if(z_get-z_set>0.5||z_get-z_set<-0.5)//电机PID% ?0 t0 b1 x% e" b# B. x( S5 v
- {1 h' c) i, A9 R2 Z: J; i" h
- LED1=0; //调节时灯亮
0 s- A2 K" y( O! }9 Q" Y - PID_val_motor=PID_realize(z_set,z_get,0);$ Q$ O% g* m2 j7 n. b' t
- PID_val_motor=PID_val_motor/10.0;9 U: m* d5 t5 I( F# r5 m, O% r! A
- if(PID_val_motor<=0): k. I0 M! S' B" I$ l6 t0 P* b
- motor_flag=0;//motor_flag控制电机正反转,PID_val_motor用于改变占空比,范围0~1' _* o. J& O0 v" J2 s, g# z
- if(PID_val_motor>0)- Z( h8 l- B$ `4 X; I" ?
- motor_flag=1;
) l G7 ?& Q! B - PID_val_motor=abs_float(PID_val_motor);
* p- m0 ?) M/ j0 _9 z. p, ` - if(PID_val_motor>2)PID_val_motor=0;//标志太远,让车停止
# R. C' j3 b4 b- h4 a6 Q - if(PID_val_motor>1&&PID_val_motor<=2)PID_val_motor=1;, A5 E( j& a3 m" B& B2 _" f1 G
- if(PID_val_motor<0.2)PID_val_motor=0;
5 ^3 s; p. P: X Z - }
# X U8 d7 X% G9 |) A) K- a - if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵机PID0 t, l. r# a2 K) c. h/ q
- {
+ a+ q- S" p# {# t' V8 R' D- U( C - LED1=0;
) ^ E7 w% f7 X% ~1 m8 m9 n8 A - PID_val_servo=PID_realize(x_set,x_get,1);, o; ~. M. z! u& @3 f! S- \
- servo_angle=((140-35)/6)*PID_val_servo+35;//线性映射,把PID的值转化为角度35~140的舵机转角
% g+ p! m' D/ J Y4 R - if(servo_angle<35)servo_angle=35;& l: W3 T$ p0 R7 [2 o5 H- J
- if(servo_angle>140)servo_angle=140;8 q3 G: b: D3 A' U
- }! Y) ?3 S; P1 f
- LED1=1;</font>
复制代码 定时器TIM2中断里改变占空比:% S5 \9 ~4 \7 D+ Q) M" P
- <font face="微软雅黑" size="3">
* i. h/ O7 _+ a1 c7 B - void TIM2_IRQHandler(void)
. V0 I2 w; A3 r4 `4 o8 Q; U. |% y& i - { 9 _* D9 R# ~3 x8 x$ v; f* J
- if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中断
# Y( `) i: E' h0 A% m) k - {
4 o. W& u* j9 T - if(motor_flag==1)//反转
" q+ H7 K7 S$ q0 S - {
) Q j& O2 j/ D0 O3 Y, V% n - TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较,来设置占空比,引脚:PC6, Y1 g8 T4 d2 H" v/ g
- TIM_SetCompare2(TIM8,0);
, T$ l' X$ D7 N# F - }
0 {1 \4 y, e& L' l - if(motor_flag==0)//正转
7 C' p0 h6 R/ A$ b; z# `3 P" m/ R - {4 {4 S! [- h) F2 L% C, W
- TIM_SetCompare1(TIM8,0);
2 q% \# k- Y$ F - TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定时器的自动重装载值进行比较,来设置占空比,引脚:PC7
, Z" f! ]( D C+ K5 L1 L3 R) i7 M9 i - }
6 U) s, G5 C* Z) j3 m$ n6 m - TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//设置舵机角度,根据舵机手册得到占空比与转角的关系,引脚:PA6
: q( x0 q1 w# E+ _# ~4 z! O& U( p - }0 f( u2 u8 x& S
- TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中断标志位$ c6 y/ @$ F& R+ A _2 |) D; A
- }
, _, P" h# z! u& k/ b8 b - </font>
复制代码
1 p! }$ Q4 h- _9 O7 v5 y$ H8 i1 s% d5 P: Z$ k C7 S# x U
. [! Z6 t0 |: U/ O |