通常的机械臂都是由多路舵机组成,我使用的是某宝上(并不)常见的五自由度机械臂。尽管商家称它为六自由度。
' Y( _+ }- H* w4 }. Q这里使用STM32F407VGT6的6路PWM输出通道来控制6个舵机的运动,树莓派(上位机)通过USB转TTL模块与STM32进行串口通讯
" f0 h j9 G d8 p PWM舵机控制原理+ o, d& |) S% p; T7 |9 c, _
" o. b) j% ^- [; W0 [
标准的 PWM 舵机有三条控制线,分别为:电源、地及信号线。
9 b4 [; U/ e6 I3 { v3 q6 O
$ ?: [; v) p5 N o& x) B Z) \4 H
, @7 C" v: }4 W5 n' N市面上大多数180°舵机需要的PWM波周期通常为20ms,高电平接收时间通常为0.5 ~ 2.5ms,对应舵机旋转角度为0 ~ 180°。用PWM波控制舵机时,只需将时钟的周期设置为20ms(50Hz),并且通过改变比较值pulse改变PWM波的高电平时间来控制舵机旋转的角度.
: y8 |: [$ B# w STM32CubeMx主要配置
$ T [- O+ X* {: G% v$ G9 s对于STM32,我使用的是STM32CubeIDE + Mx 进行开发。时钟框图为默认配置,如图。 . v- w" ?. a8 F: w1 x0 G
: @5 M& V/ Q4 y
8 Q$ [4 H) {3 V$ _' x( C9 ], t TIMER
+ B# B( l0 q+ t' I由于需要控制6个舵机,我选择了TIM3(4路PWM输出)和TIM9(2路PWM输出)。
% h% l% K7 H) JPWM输出的频率由时钟APB2决定,由时钟框图可知此处的APB2频率为16MHz;PWM波频率计算公式为: W = APB2 / (PSC + 1)(ARR + 1) 其中PSC为分频系数,ARR为自动重装载值。这里我将PSE设置为39,ARR设置为7999。
/ j* p( w, \0 A; R8 ^( y: x8 j6 M6 s9 p9 G6 j3 s- c
6 ^8 V; C8 o4 l1 }( @
注意,这里的计数模式(Counter Mode)不同时,会导致接下来比较值值相同时舵机的旋转方向不同。
由于PWM波高电平时间需控制在0.5 ~ 2.5ms内,所以各PWM通道的比较值(pulse)必须控制在200 ~ 1000之内; (8000 x 0.5 / 20 = 200)(8000 x 2.5 / 20 = 1000) 当舵机旋转角度为90°时,pulse值应为600。这里我将各个PWM输出通道的比较值均设置为600。 2 a( L: ?$ r' V0 E* F6 K c
9 V9 L8 v) s5 p) s9 S
由于我手头上的机械臂中的一个舵机用于控制机械爪,其为90°舵机,所以在配置控制该舵机的PWM通道时,比较值范围仅能为200 ~ 600,中间值为400我使用的单片机为STM32F407VGT6,其TIM3和TIM9所对应的PWM输出通道分别为PA6, PA7, PB0, PB1和PE5, PE6。
" R) k) l0 }& @5 L# D" I7 }这里只需将这些IO口设置为复用推挽输出以及上拉即可。
% n- i* ?, _! g
- h* K4 @( t: y; [6 ?. S1 {, N/ U1 ~& m# S) s, e
串口配置
1 V+ a+ }, d3 q7 a% x7 l7 @, u+ D这里我选择USART_2用作串口通讯,其Tx与Rx分别对应为PA2、PA3。 5 O4 d# [6 g& v' k' A9 o w6 h1 J
串口的配置如图所示。
. m- e, \' O9 l, c/ `0 i) l" y! W V
0 u8 [( d3 e3 v) t
这里使用异步通信模式,注意波特率等参数需与上位机相匹配。6 Z" G7 g& Y( e6 |4 q
) i' Q: h( u& Z* |# x+ l
" T& p- y9 c, ~% T) k
PA2与PA3均设置为复用推挽输出,上拉。
6 M' G% q, x& F3 Q5 A1 ~' j 中断控制; e6 ~) l/ O$ n/ W& t2 N
由于这里我只使用了串口接收中断,所以NVIC这里可以不用配置;若STM32除了控制机械臂外还有其余任务,则可能需要配置NVIC。 8 X* c7 h; v* I. c
STM32CubeIDE代码实现5 \6 h& k# i8 d5 t# i# _
在STM32CubuMx配置完毕并生成工程之后,我们的主要任务只有设计上位机与单片机的通讯协议,以及完成串口接收中断函数即可。 ) f6 ~( S) q. M3 c- Z
通讯协议设计: ~: v; R2 |/ v, I( L+ i; ?
我初步的设计为串口发送一次数据,控制单个舵机的角度;所以发送的数据中需要包括舵机的编号以及舵机的目标角度(或者由角度转换而成的比较值)。 在这里我没有设计通讯头以及对舵机转速的控制,因为这里我的串口只用于传输控制舵机的指令,以及我使用的是小型舵机,其转速并不是很快,可以满足我的要求,无需控制其速度。 设计思路如下:
7 G1 k# ?! V: C- |4 N2 [上位机每次发送16位的数据(2个uint8_t类型,串口只能发送串口),舵机编号为0~5,发送的pulse值为 (200 ~ 1000) - 200,将舵机编号数值乘以1000加上pulse值减去200得到一个uint16_t类型的数值;再将其转换为十六进制进行发送。 例:对3号舵机进行操作,角度为45°。 数据处理:8 N+ @' X: b! F. [7 ^% t
3 x 1000 + (45° / 180°) x 800 = 3200;
. o+ m% ` X. K! X( Y% g3200转换为十六进制为:0x0C80。 STM32进行逆向操作得到原数据。 " `- B2 ~7 q! y! T( ]4 H
STM32代码实现由于串口发送的是两个uint8_t类型的数据,所以在解码之前还需将两个uint8_t类型数据转换为一个uint16_t类型的数据。 在打开USART2的接收中断时,设置为接受到两个uint8_t类型的数据进入接收中断,以便于在接收中断处理函数中进行数据处理。
; D) d0 H! _( T/ ~4 X8 E) }- uint8_t pulse[2];
' A3 k6 ?/ p6 G - HAL_UART_Receive_IT(&huart2, pulse, sizeof(pulse));
复制代码
; l* Y& N* o; V L/ [. K W+ ?
8 y; H$ t5 h. g
串口接受中断函数代码如下:
" ~: Z# c) V% o9 {$ O3 \/ [9 ^
- void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)) t( ~+ G7 a/ v/ q7 B1 H- B
- {
$ e7 k/ S0 A3 L: L: V - if(huart->Instance == USART2), H, C' @: A2 r% T+ }8 e( u# t" a
- {
! G) x9 l9 @/ b1 }$ T9 g - uint8_t rec0, rec1;! t8 y! e8 w$ j# R9 s' v
* k" K5 u2 \8 u) R8 F$ ~- // 获取接收到的两个字节的数据
9 N: p4 @8 \4 \8 k( e - rec0 = *((huart->pRxBuffPtr) - 2);
, A+ _* L! s0 R/ {, D - rec1 = *((huart->pRxBuffPtr) - 1);
1 i" b2 D5 W$ y# Y! [- A - # J# g# W( G5 m& ?6 E+ P6 X) R
- uint16_t numPart[2], armControl, arm targetPulse;
+ m7 @5 U$ o+ ]' S- [2 ] D, | - 2 ^1 y: G8 V/ p$ t' l
- // 通过移位与强制转换得到uint16_t类型数据
. Q: K* {) e9 h; | - numPart[0] = (uint16_t) rec0;' l" n% C& j. r4 b1 B
- numPart[1] = (uint16_t) rec1;; o5 A& d4 g/ K, S
- armControl = ((numPart[0] << 8) | numPart[1]);
3 ?! T! {7 q7 Y* y - : h& g. U6 t; `0 D6 f* ~
- // 逆向解码% [6 O! W! f6 \8 a& U! f
- arm = armControl / 1000; // 舵机编号
' `2 b5 ?1 N) q - targetPulse = armControl % 1000 + 200; // 目标比较值
# \' h4 u e* K/ W! H1 J# H
_! q* E9 i2 l! ] V9 R- // 通过switch语句改变指定PWM通道的比较值
: U1 X8 C1 I8 D+ m5 l7 f8 E0 O6 {; [ - switch(arm); G" x( i+ y+ b* A+ B2 q- z3 _
- {
y# P9 \: x' B) P: Q - case 1:
" ~+ b8 p; e W$ Q. b2 T - __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, targetPulse);0 M W5 `4 N ?0 t! M& U
- break;. M( s0 s/ |: q, ^0 Q4 o" i
/ F4 v k. \# ]: @9 z4 e! T- case 2:" X/ I2 C7 Y# n% u8 K
- __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, targetPulse);+ t& ~# n; ]! T4 X: d; I$ }
- break;( W# u1 t' F7 H' U( x
5 V g# c! n5 J$ K2 r5 p9 k- case 3:5 g: A2 D1 y! u4 k3 V
- __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, targetPulse);; E0 _ n, b2 E$ w1 X7 I
- break;
! {2 |, t7 i% o" i
& Y A6 i# z7 k1 B- case 4:, s9 g+ C8 S& z8 @
- __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, targetPulse);" E( [% d7 m, z9 w# H6 H0 R4 U7 o
- break;
4 v* h, d+ v Q+ u. V7 ?
& X# I g# b/ |6 ]- case 5:4 d2 Z( q& ^) Z3 o( |; ?0 G1 P
- __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_1, targetPulse);& W y( j; P) x7 P: }
- break;1 I9 N4 Y* M: [' W4 D
- * s, J: r$ A* v4 d
- case 6:+ \0 X& R7 n0 ]% E( g7 H
- __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_2, targetPulse);
0 I0 a% q9 N7 \ - break;
( B7 \. x7 T7 i @. y8 [ - }/ e* f! d+ ^& a6 X Q( D, K0 H5 G
- }0 V9 |4 v3 k/ K2 k( H
- }5 U# O) u5 K- Q' c
复制代码
/ e K0 y0 m. ^在进入while循环之前还需将PWM通道使能;
0 w/ G: i. b( `0 h+ G3 U- HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);3 V% j( M" z) b- j
- HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);; M8 D/ l, _+ ]. h3 d; A& a( B) X
- HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
% j# S8 J4 ~! V2 [* Y9 W - HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);$ l3 X! k& ?. _3 E! s! ?
- HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_1);" O2 z Q3 V% ?1 r; n9 [
- HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_2);
复制代码
0 t( L$ k! g H- P9 M0 K
- [& G, X+ u; y2 b
并且在USART2_IRQHandler函数中使能串口接收中断:
1 K) H$ U( d/ f! t) C# [
- void USART2_IRQHandler(void); L; d0 y- f% b# \* a
- {2 |. Y% P& u5 |3 @: }
- /* USER CODE BEGIN USART2_IRQn 0 */
5 Q: ?4 r: _- K# K& c4 V - X; E8 g8 w7 P0 N- l( R. O% j
- /* USER CODE END USART2_IRQn 0 */" B: n" K9 |: a- N
- HAL_UART_IRQHandler(&huart2);4 U& C9 K# M' A( N, B3 T
- /* USER CODE BEGIN USART2_IRQn 1 */
% M' X4 U1 @* V: k& j/ V) ^& P' x - HAL_UART_Receive_IT(&huart2, pulse, sizeof(pulse));
0 n3 J9 K! S& ]& B" R* \ - /* USER CODE END USART2_IRQn 1 */9 r8 n6 B: G# [( J7 B6 B
- }
6 X8 V2 g/ ?* n. B6 e& V0 |/ G
复制代码
% S( g4 G1 X7 M2 y2 {1 C7 L% [# r# O$ m7 I5 z5 s# M
测试
/ Q" `( ^" Q/ f9 n0 P在测试时我使用的环境是安装有Ubuntu 18.04的树莓派,串口调试工具为CuteCom;由5V锂电池为单片机与舵机(机械臂)供电,每次手动发送两个字节的数据,十进制转十六进制在科学计算器上进行。机械臂的运动符合预期。 ! Z) \: A) A( p, x2 Z) o. ]: F
经过测试,同时发送12个字节的数据,机械臂也能够正常运行。 3 W; Q! t6 Z1 y$ H1 N8 i$ }
这里需要注意,在接线时,舵机的地线必须与单片机共地。 3 S! `9 S/ x# k; P5 h4 r
之后就可以将上位机串口发送的代码写入C++或者Python程序,通过上位机控制机械臂。 ' C3 f% C1 C3 v, ?/ T- e. K
# Z: b9 d/ w( U |