这款双轴机械臂是应用于实验室中进行自动样品转换的执行机构。为了能够在一定范围内将试剂瓶(试管) 完成自动搬运,需要完成对样品的抓取,提升和放下、水平移动(二维) 等功能。使用带有肘关节、肩关节以及上下移动关节的机械平台可以完成这项设计。
在基于STC8H1K28双轴机械臂驱动模块 给出了对肩关节、肘关节上的步进电机、角度传感器读取和控制模块的设计。本文下面就是利用该模块对机械臂进行运动控制。
▲ 双轴机械臂的外观
(1) 步进电机引线
引出4PIN的接口,分别定义为:
PIN | 符号 | 功能 | 颜色 |
---|---|---|---|
PIN1(MARK) | PU+,DR+,MF+ | 三个控制端口的公共电源线 | 红,蓝,棕 |
PIN2 | PU- | 脉冲输入 | 黄 |
PIN3 | DIR- | 方向输入 | 黑 |
PIN4 | MF- | 势能输入 | 绿 |
上述定义的四线插头可以接入基于STC8H1K28双轴机械臂驱动模块中的PANGx(角度传感器端口)。
(2) BH38角度传感器接口
引出4PIN接口,定义为:
PIN | 符号 | 功能 | 颜色 |
---|---|---|---|
PIN1(MARK) | +5V | 工作电源 | 棕:BROWN |
PIN2 | 0V | 电源地 | 蓝:BLUE |
PIN3 | A+ | 485总线A+ | 黑:BLACK |
PIN4 | B- | 485总线B- | 白 |
另外两个引线置零(RED),编程(黄)不再接入端口。
(3) 通道分配
在基于STC8H1K28双轴机械臂驱动模块1具有两个通道的步进电机控制和角度传感器。将通道1(步进电机控制、角度传感器读取)分配给肩关节;将通道2(步进电机控制,角度传感器读取)分配给肘关节。
按照上述的通道方案,将肘关节、肩关节的部件电机驱动端口、角度传感器端口分别进入实验电路板的相应的端口。具体参见上面的“通道分配”中的示意图。
▲ 控制模块与关节步进电机和角度传感器相连
▲ 连接有WiFI调试下载器的系统
在 STC8K28步进电机ULC2003触发器 测试两个步进电机需要供电电压大于15V,这里使用电源24V开关电源进行供电。
▲ 供电电源:24V,+5V
电源同时提供+5V,+24V 电压。供电电流最大4.6A。
使用ADJ电位器可以同时提高和降低输出的+5V, +24V电压。
直接利用在基于STC8H1K28双轴机械臂驱动模块中的串口命令,通过WiFi调试界面发送指令控制双轴机械臂运动。
通过串口命令step2实现肘关节的运动:
step2 2000
step2 -2000
step2的命令参数: n > 0 n > 0 n>0:肘臂顺时针旋转; n < 0 n < 0 n<0肘臂逆时针旋转。(从顶视图来看。)
▲ 肘关节运动
step1 1000
step1 -1000
step1的参数: n > 0 n > 0 n>0肩部顺时针旋转。 n < 0 n < 0 n<0逆时针旋转;(从顶视图来看)。
▲ 肩关节运动
通过上面肩关节和肘关节运动情况来看,证明了以下两方面的事实:
▲ 肩关节运动对应的角度变化数值
▲ 肘关节运动对应角度变化
使用angle命令读取肘关节与肩关节的角度信息,使用step1(肩关节运动命令),step2(肘关节运动命令)驱动两个运动,可以证明:
通过将肩关节和肘关节的电机使能取消,手动将肩关节和肘关节旋转的极限位置。使用串口命令angle,可以获得在各个极限位置下角度的数值。
▲ 肘关节的角度变化范围
▲ 肩关节角度范围
脉冲与角度之间的比值。使用move1(move2)命令,然后测量angle的变化。
■ 表格:move 10000前后的角度变化
肩关节角度 | 肘关节角度 |
---|---|
10781 | 3697 |
14851 | 7773 |
肩关节变化 | 肘关节变化 |
4070 | 4076 |
■ 表格:move 10000前后的角度变化
肩关节角度 | 肘关节角度 |
---|---|
14851 | 7773 |
2556 | 11889 |
肩关节变化 | 肘关节变化 |
4033 | 4116 |
对于肩关节角度: A s t a r t = 10781 A_{start} = 10781 Astart=10781, A s t o p = 2556 A_{stop} = 2556 Astop=2556。考虑到其中的角度可能经过了16328( 2 14 2^{14} 214),所以,实际角度变化范围: Δ A = 2 14 − A s t a r t + A s t o p = 8159 \Delta A = 2^{14} - A_{start} + A_{stop} = 8159 ΔA=214−Astart+Astop=8159
对于肘关节: A s t a r t = 3697 A_{start} = 3697 Astart=3697, A s t o p = 11889 A_{stop} = 11889 Astop=11889。实际角度变化: Δ A = A s t o p − A s t a r t = 11889 − 3697 = 8192 \Delta A = A_{stop} - A_{start} = 11889 - 3697 = 8192 ΔA=Astop−Astart=11889−3697=8192
通过上述分析,可以看到两个角度基本上都是旋转了 2 13 = 8192 2^{13} = 8192 213=8192,也就是半圆的角度。对应的移动角度为20000。
查看两个步进电机的设置,SW2,3,4,5=0000( 42HS48EIS ),电机旋转一周为40000步。所以上述分析与电机的设置是相符合的。
使用命令move12移动20000步。
肩关节角度 | 肘关节角度 |
---|---|
2625 | 11971 |
12617 | 5562 |
角度差 | 角度差 |
6392 | 6409 |
运动20000步,对应的角度为:
20000 51200 × 360 = 140.625 {{20000} \over {51200}} \times 360 = 140.625 5120020000×360=140.625
对应的14bit的角度为:
20000 51200 × 2 14 = 6400 {{20000} \over {51200}} \times 2^{14} = 6400 5120020000×214=6400
从上面的数据可以看到实际测量的角度与理论值之间相差 ± 10 \pm 10 ±10之内。
将42HS48EIS步进细分设置为:51200,转动与角度只比:
Δ A n g l e : Δ S t e p = 2 14 51200 \Delta Angle:\Delta Step = {{2^{14} } \over {51200}} ΔAngle:ΔStep=51200214
★ 在调试过程中,肘关节电机输出与机械部分滑脱了,现在已经无法带动大臂旋转。
上述的试验故障,最终通过白超的实验,验证应该数据步进电机的角度偏差范围超出范围决定的。这个故障可以通过步进电机上的指示灯闪烁的模式来确定。
闪烁5次停3秒:表示控制偏差超出范围。
▲ 肘关节运动180°对应的角度变化
▲ 肘关节运动
▲ 肩关节运动180°对应角度传感器变化值
▲ 机械臂肩部 运动
显示了对于肩关节角度传感器在运动范围之内有一个角度突变的过程。这是由于角度传感器量程具有有限的bit。
为了便于在程序里判断,对于肩关节的角度做如下的修正:
假设原来的肩关节的角度值: S o r i g S_{orig} Sorig,那么修正之后的数值为: S n e w = m o d ( S o r i g + 6000 , 2 14 ) S_{new} = \bmod \left( {S_{orig} + 6000,\,\,2^{14} } \right) Snew=mod(Sorig+6000,214)
▲ 肩关节角度修正之后的变化规律
修正后的肩关节的变化就没有突变了。
#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST1.PY -- by Dr. ZhuoQing 2020-08-22
#
# Note:
#============================================================
from headm import *
from tsmodule.tsstm32 import *
angledim = []
for i in range(100):
stm32cmd('CLEAR')
stm32cmd('step1 -256')
time.sleep(1)
stm32cmd('angle')
time.sleep(0.2)
angle = stm32memo(1)
angledim.append(angle[0])
printff(i, angle[0])
tspsave('Angle1', angle=angledim)
printf('\a')
plt.plot(angledim)
plt.xlabel("Step")
plt.ylabel("Angle")
plt.grid(True)
plt.tight_layout()
plt.show()
#------------------------------------------------------------
# END OF FILE : TEST1.PY
#============================================================
将基于STC8H1K28双轴机械臂驱动模块与双轴机械臂组装,进行了初步的调试。需要进行如下的改动:
控制模块AD工程文件:AD\XQWF\2020\机械臂\CNT2AXESSTC8H.PcbDoc ↩︎
单片机C51控制程序:C51\STC\Project\XQWF\2020\STC8H2AXES\STC8H2AXES.uvproj ↩︎