通常的机械臂都是由多路舵机组成,我使用的是某宝上(并不)常见的五自由度机械臂。尽管商家称它为六自由度。
! s7 e9 K& \+ t这里使用STM32F407VGT6的6路PWM输出通道来控制6个舵机的运动,树莓派(上位机)通过USB转TTL模块与STM32进行串口通讯 0 X4 v ]- U% Q7 @: W
PWM舵机控制原理8 A& k/ P( U- v r2 e! f
4 J; ], Q/ y5 C1 k$ [
标准的 PWM 舵机有三条控制线,分别为:电源、地及信号线。
) \1 s A6 l2 ?+ F8 h2 j3 [- {
V' P1 B& @! ~2 o; P: ~( R) |2 R7 o6 S$ v: _
市面上大多数180°舵机需要的PWM波周期通常为20ms,高电平接收时间通常为0.5 ~ 2.5ms,对应舵机旋转角度为0 ~ 180°。用PWM波控制舵机时,只需将时钟的周期设置为20ms(50Hz),并且通过改变比较值pulse改变PWM波的高电平时间来控制舵机旋转的角度. : N! Z* n3 n1 a0 \! T
STM32CubeMx主要配置% e6 G- b1 g2 A( |1 ?7 P0 `1 {$ l5 P
对于STM32,我使用的是STM32CubeIDE + Mx 进行开发。时钟框图为默认配置,如图。 ( D$ M; ~+ n6 K1 w$ U
9 o& N% |! N* D" U- v
5 d$ ~: V# i, v" l$ v* }8 E1 ~ TIMER2 y1 w( J2 g. i8 F! W% T/ V3 z$ T
由于需要控制6个舵机,我选择了TIM3(4路PWM输出)和TIM9(2路PWM输出)。
5 C& r! u: `1 [PWM输出的频率由时钟APB2决定,由时钟框图可知此处的APB2频率为16MHz;PWM波频率计算公式为: W = APB2 / (PSC + 1)(ARR + 1) 其中PSC为分频系数,ARR为自动重装载值。这里我将PSE设置为39,ARR设置为7999。 % S" n/ A4 c( z8 r: N9 ?. ~6 Z2 p* Z- Y
2 F6 z! ]- e& D5 H
. j* S% k/ Y8 b注意,这里的计数模式(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。 ; |8 b- }/ P6 |6 X
5 h$ [+ ~) u* i; z* `由于我手头上的机械臂中的一个舵机用于控制机械爪,其为90°舵机,所以在配置控制该舵机的PWM通道时,比较值范围仅能为200 ~ 600,中间值为400我使用的单片机为STM32F407VGT6,其TIM3和TIM9所对应的PWM输出通道分别为PA6, PA7, PB0, PB1和PE5, PE6。
( Z2 H- A7 ~* p这里只需将这些IO口设置为复用推挽输出以及上拉即可。
. U* h2 g& {( ]+ C- z3 b. q4 p
\. _7 J$ _% T
0 y( G3 P% I6 T M- B+ }1 k5 K 串口配置
- o b1 }( t2 q. |这里我选择USART_2用作串口通讯,其Tx与Rx分别对应为PA2、PA3。
! K! E% K1 w: s
串口的配置如图所示。 1 V; Q& h/ D3 T$ G$ R, l
5 y# Z9 Z2 B6 `, ?( V% z7 }
* |( f6 b% w A这里使用异步通信模式,注意波特率等参数需与上位机相匹配。
, s9 u% X$ m |; u& |# h ; p6 ]/ s" m. x' d
9 s) L( T* R$ n' o+ \; O
PA2与PA3均设置为复用推挽输出,上拉。
4 ~- z. |9 D$ t! C' n' I 中断控制
0 l- P: o' L7 T& i由于这里我只使用了串口接收中断,所以NVIC这里可以不用配置;若STM32除了控制机械臂外还有其余任务,则可能需要配置NVIC。 ! V# w6 m7 h6 n5 T m
STM32CubeIDE代码实现1 B% `/ ?+ V+ u! q/ ~
在STM32CubuMx配置完毕并生成工程之后,我们的主要任务只有设计上位机与单片机的通讯协议,以及完成串口接收中断函数即可。 / c( [0 Z' {& N( l7 I+ G1 k# \' k
通讯协议设计7 O6 R- \* K# K2 C u: H
我初步的设计为串口发送一次数据,控制单个舵机的角度;所以发送的数据中需要包括舵机的编号以及舵机的目标角度(或者由角度转换而成的比较值)。 在这里我没有设计通讯头以及对舵机转速的控制,因为这里我的串口只用于传输控制舵机的指令,以及我使用的是小型舵机,其转速并不是很快,可以满足我的要求,无需控制其速度。 设计思路如下: }& i/ a# H- n% e: \0 F/ P7 d
上位机每次发送16位的数据(2个uint8_t类型,串口只能发送串口),舵机编号为0~5,发送的pulse值为 (200 ~ 1000) - 200,将舵机编号数值乘以1000加上pulse值减去200得到一个uint16_t类型的数值;再将其转换为十六进制进行发送。 例:对3号舵机进行操作,角度为45°。 数据处理:
+ Y9 C6 C$ E( O! Y0 v5 c3 x 1000 + (45° / 180°) x 800 = 3200;# Z% u' n( z$ t4 E8 J% j0 d
3200转换为十六进制为:0x0C80。 STM32进行逆向操作得到原数据。 1 A! z& `8 w) A$ w+ P4 o4 b& g8 ]
STM32代码实现由于串口发送的是两个uint8_t类型的数据,所以在解码之前还需将两个uint8_t类型数据转换为一个uint16_t类型的数据。 在打开USART2的接收中断时,设置为接受到两个uint8_t类型的数据进入接收中断,以便于在接收中断处理函数中进行数据处理。
4 h" }$ m4 E3 u5 z- uint8_t pulse[2];5 f- D; r6 x2 i' _
- HAL_UART_Receive_IT(&huart2, pulse, sizeof(pulse));
复制代码 6 [2 r. W& q# {5 G o6 M
- |3 F; B- f3 R
串口接受中断函数代码如下: , W4 t4 ~$ N# W. h+ V" e) @
- void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
$ K1 B% b S* g* v/ A - {# }, M3 Q* P- |4 V3 `2 Q/ ]4 _
- if(huart->Instance == USART2)
) z- J) v: u7 m2 T* A, {+ j3 D5 N - {$ F( ?0 K1 Q7 e" w# F: y2 F
- uint8_t rec0, rec1;) w6 _7 Q" Q& }
7 G" q7 Q1 f7 {# E- [- // 获取接收到的两个字节的数据
2 L; i6 i/ v' h& \$ c/ y& k- \ - rec0 = *((huart->pRxBuffPtr) - 2);& A4 @3 B6 A0 N1 d* m
- rec1 = *((huart->pRxBuffPtr) - 1);8 E7 Z, |( L% u3 W) m, v2 a& B
* ]9 K7 B5 V6 N- Q* i( _- uint16_t numPart[2], armControl, arm targetPulse;' h8 \3 ]/ b7 y
- 9 t. I7 i! g) }5 l4 w% N
- // 通过移位与强制转换得到uint16_t类型数据
1 I0 F5 a0 x' y$ u - numPart[0] = (uint16_t) rec0;% G0 B! {7 U4 n( ~, \3 w
- numPart[1] = (uint16_t) rec1;" h4 @, W- ] y* K; W8 `
- armControl = ((numPart[0] << 8) | numPart[1]);: b- y8 [8 N' N9 a% F7 S2 M
" \9 P" @* n, m* f! f- // 逆向解码
8 a0 k1 p: \' o0 t" X" @# H9 S ` - arm = armControl / 1000; // 舵机编号. J r- d8 D0 \4 B b
- targetPulse = armControl % 1000 + 200; // 目标比较值# l( R/ _# P' Q: T
, v) [$ ^6 H2 A7 `" w) h. @- // 通过switch语句改变指定PWM通道的比较值
# o; b% m* e% e4 u& u# v - switch(arm)& ]( [& y7 ]2 k v0 b' {' D) I
- { t+ B. B& O7 T7 N/ q2 d5 a5 P
- case 1:- z" o% g% ~. P6 R ], |
- __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, targetPulse);
' w, R9 Y' e, L F# @% p, { - break;" f' N% |6 A7 Z X
- , R$ d( b# G* I- i5 C! K
- case 2:
, f! `1 d1 G' r1 a! \: H$ `4 U, T - __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, targetPulse);
, _5 t1 k5 J( `6 E8 s- x" ? - break;
) y' m, ^( m- S2 s- S5 @5 E
8 b1 ^8 t" z, t& b' S; c- k; W- case 3:
/ a" {$ j6 G" F3 | - __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, targetPulse);# ^' t- N6 e+ h' d$ u- l! k
- break;( S0 N4 W2 X2 h, }
" s' V4 u: z1 |) O9 ~# I: j- case 4:
& x0 @1 Y5 ~2 b - __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, targetPulse);
/ W1 B: u- ^+ w - break;+ y5 W6 R2 I* T7 v
- N( ?/ Z8 {. Y" Q# w, _$ V- case 5:
* x0 L1 L% k; y! C6 U1 I7 v - __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_1, targetPulse);* ?$ _; s2 P' P4 k8 w
- break;5 f& D( k" K' d7 H6 ]
- 8 e! T( p1 y' A4 L$ ~! [& m
- case 6:
* f A3 N( ^* Q: L/ {1 T4 z - __HAL_TIM_SET_COMPARE(&htim9, TIM_CHANNEL_2, targetPulse);
x6 B4 o( x2 [( k - break;
4 W/ h1 a7 q4 Q7 x+ |& {6 t - }
; ^+ |6 d* f4 v3 N- ` - }
7 A& d& `1 @" T% J* K - }
7 \. a7 _2 N! m* K |
复制代码 ' b1 u& O( Y# H! g
在进入while循环之前还需将PWM通道使能;
' e6 q" l( k0 ?! a: t" K- HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
7 \5 {' R2 X' f& @" }. \$ s9 M - HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
# b* d! k$ Y Y4 ~7 z - HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
# ?" a! O! i C$ h3 d/ s% \ - HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);' a- l, @: b% e4 O6 ]6 v
- HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_1);) Q: U6 X6 V6 b8 M6 g
- HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_2);
复制代码
8 I q1 A7 w) [ y1 k/ _, S
% Z0 w- B; s6 k1 f/ _3 g
并且在USART2_IRQHandler函数中使能串口接收中断: 3 c5 K$ M$ ]* B6 K! h+ S# S7 r
- void USART2_IRQHandler(void)8 s" c7 e6 B) g! M# g4 C7 \( A# @
- {
' W# @3 k1 k {0 v) P8 b' b - /* USER CODE BEGIN USART2_IRQn 0 */
4 w( g! L; ]" S0 m - * l: I& B- V0 v/ s) o
- /* USER CODE END USART2_IRQn 0 */! l2 F, T( @) d/ O
- HAL_UART_IRQHandler(&huart2);3 i4 |: B/ E/ h0 i3 t+ `
- /* USER CODE BEGIN USART2_IRQn 1 */
/ D$ H; B. q2 O: { - HAL_UART_Receive_IT(&huart2, pulse, sizeof(pulse));' a) s$ K! E1 |0 L
- /* USER CODE END USART2_IRQn 1 */9 \5 T2 j) r m$ o0 b+ l$ p
- }0 ^- G; `0 O1 |
复制代码
) W8 K& _) U& t0 q4 A
' X. i: G% [; p1 ~1 ?: V测试
2 J! ^; \6 f- K& Y* f在测试时我使用的环境是安装有Ubuntu 18.04的树莓派,串口调试工具为CuteCom;由5V锂电池为单片机与舵机(机械臂)供电,每次手动发送两个字节的数据,十进制转十六进制在科学计算器上进行。机械臂的运动符合预期。 , v! Q( i3 U. |' _# S* l
经过测试,同时发送12个字节的数据,机械臂也能够正常运行。 8 a& P7 |4 o; Q0 v5 h
这里需要注意,在接线时,舵机的地线必须与单片机共地。
7 j0 D6 V9 ?8 W( K" e2 j' ^) `4 d
之后就可以将上位机串口发送的代码写入C++或者Python程序,通过上位机控制机械臂。
1 Q/ i" x9 l I1 ^* y
$ e( }+ g* y' b. U" U$ P! H* l8 Y |