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