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