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