机械手搬运控制系统

机械手搬运控制系统

机械手工作过程如下：通电后，机械手先行复位。然后，机械手臂前伸，手开始旋转至所需位置；机械手臂下降至物品处，手张开，然后抓手夹紧物品；机械手臂上升将物品拿起；机械手臂收回，底盘旋转，底盘旋转至指定位置后；机械手臂前升，机械抓手旋转；手臂下降，手张开将物品放下；机械手臂上升，手复位。至此，一个工作周期结束。

把物体从A 点搬运到B 点，物体摆放位置如图（1）。取物过程就是机械手臂前升——底盘顺时针转动——气动抓手旋转——机械手臂下降——气动抓手抓紧——机械手臂上升——底盘逆时针转动——机械手臂下降——气动抓手松开，取物过程完成。

根据动作顺序，其流程动作如下所示：

该流程图的设计主要是根据机械手的动作要求来完成的，它要求在接通电源，程序传入后，不论机械手原来在什么位置，都先回到初始位置，为机械手开始动作做准备。系统的初始复位包括硬件复位和软件复位。其中硬件复位是指机械手臂处于左限位状态和上限位状态，机械抓手处于回位状态，底盘处于回位状态；软件复位是指系统的参数设置恢复到初始状态。当初始化完成后（这是以机械手碰到限位后为标志），横轴前升，手开始旋转至所需位置，手张开，竖轴下降至物品处，手加紧物品，竖轴上升将物品拿起。横轴收回，底盘旋转；横轴前升，手旋转；竖轴下降，手张开将物品放下；竖轴上升，手复位。至此，整个机械手的动作全部结束。

，系统IO 分配表如下：

输入接口			输出接口
PLC 端	单元板端口	注释	PLC 端	单元板接口	注释
I0.0	SQ3	旋转码盘脉冲输出	Q0.0	U 轴-CP	水平轴脉冲信号
I0.1	SQ4	L 轴原点（水平方向）	Q0.1	L 轴-CP	垂直轴脉冲信号
I0.2	SQ7	U 轴限位（垂直方向）	Q0.2	U 轴-DIR	水平轴方向信号
I0.3	SQ5	L 轴限位（水平方向）	Q0.3	L 轴-DIR	水平轴方向信号
I0.7	SQ6	U 轴原点垂直方向）	Q0.4	S 轴-F	底盘顺时针转动
I1.0	SQ1	T 轴原点（手臂）	Q0.5	S 轴-R	底盘逆时针转动
I1.1	SQ2	T 轴限位（手臂）	Q0.6	T 轴-F	手臂顺时针转动
I1.2	SQ8	S 轴原点（底盘）	Q0.7	T 轴-R	手臂逆时针转动
I1.3	SQ9	S 轴限位（底盘）	Q1.0	B 轴-YV	气动夹手

2.接线原理图

MIAN:OB1
Network 1 //初始化
LD SM0.1
CALL SBR31
CALL SBR36
CALL SBR2

Network 2 // 限位
LD SM0.0
CALL SBR1

Network 3 // 步进电机的坐标
// 步进电机的坐标(mm)
LD SM0.0
MOVR 120.0, VD100
MOVR 110.0, VD104
MOVR 100.0, VD108
MOVR 110.0, VD112

Network 4
LD SM0.1
S S0.0, 1
R S0.1, 10

Network 5
LSCR S0.0

Network 6 // 网络标题
LD SM0.0
CALL SBR0
CALL SBR2

Network 7
LD I0.7
A I0.1
A I1.0
A I1.2
SCRT S0.1

Network 8
SCRE

Network 9
LSCR S0.1

Network 10
LD SM0.0
TON T121, 5

Network 11
LD T121
EU
CALL SBR30, 1000.0, 3000.0, VD100

Network 12
LD T121
A SM66.7
SCRT S0.2

Network 13
SCRE

Network 14
LSCR S0.2

Network 15 // 入库
LDN I1.1
A SM0.0
S Q1.0, 1

Network 16
LD I1.1
EU
CALL SBR35, 1000.0, 3000.0, VD108

Network 17
LD I1.1
A SM76.7
TON T37, 5
A T37
SCRT S0.3

Network 18
SCRE

Network 19
LSCR S0.3

Network 20
LD SM0.0
S Q1.4, 1
TON T122, 5

Network 21
LD T122
EU
CALL SBR35, 1000.0, 3000.0, 10.0

Network 22
LD T122
A SM76.7
SCRT S0.4

Network 23
SCRE

Network 24
LSCR S0.4

Network 25
LD SM0.0
TON T123, 5
TON T146, 6

Network 26
LD T123
EU
CALL SBR30, 1000.0, 3000.0, 10.0

Network 27
LD SM66.7
A T146
AN I1.3
S Q1.2, 1

Network 28
LDD>= HC0, 8
LPS
A SM66.7
SCRT S0.5
LPP
R Q1.2, 1

Network 29
SCRE

Network 30
LSCR S0.5

Network 31
LD SM0.0
TON T124, 5

Network 32
LD T124
EU
CALL SBR30, 500.0, 3000.0, VD104

Network 33
LD T124
A SM66.7
TON T38, 5
A T38
SCRT S0.6

Network 34
SCRE

Network 35
LSCR S0.6

Network 36
LDN I1.0
A SM0.0
S Q1.1, 1

Network 37
LD SM0.0
TON T125, 5

Network 38
LD T125
EU
CALL SBR35, 500.0, 3000.0, VD112

Network 39
LD T125
A SM76.7
TON T39, 5
A T39
SCRT S0.7

Network 40
SCRE

Network 41
LSCR S0.7

Network 42
LD SM0.0
TON T126, 5

Network 43
LD T126
R Q1.4, 1

Network 44
LDN Q1.4
LPS
A T127
SCRT S0.0
LPP
TON T127, 10

Network 45
SCRE


子程序` initialize:SB0 //初始化复位
TITLE=子例行程序注释
BEGIN
Network 1 //U 轴初始化复位
LD I0.1
AN I0.7
A SM66.7
CALL SBR30, 800.0, 2000.0, -3800.0

Network 2 //L 轴初始化复位
LDN I0.1
A SM66.7
CALL SBR35, 800.0, 2000.0, -3800.0

Network 3 //T 轴（手臂）复位
LD I0.1
AN I1.0
S Q1.1, 1

Network 4 //S 轴（底盘）复位
LD I0.1
AN I1.2
S Q1.3, 1


子程序limit:SBR1 //限位保护
Network 1
// 手臂原点限位
LD I1.0
R Q1.1, 1

Network 2 //手臂正转限位保护
LD I1.1
R Q1.0, 1

Network 3 //底盘原点限位
LD I1.2
R Q1.3, 1

Network 4 //底盘正转限位
LD I1.3
R Q1.2, 1

Network 5 //U 轴限位保护
LD I0.7
EU
LD I0.2
EU
OLD
CALL SBR31

Network 6 //L 轴限位保护
LD I0.1
EU
LD I0.3
EU
OLD
CALL SBR36


子程序hsc0:SBR2 //高速计数器
Network 1 // 网络标题
LD SM0.0
MOVB 16#F8, SMB37
HDEF 0, 0
MOVD 0, SMD38
HSC 0


子程序pls_ch0:SBR30 //控制脉冲发生器PTO0
Network 1 // 当目标值与当前值一致时，停止动作
LDR= LD8, VD4200
CRET

Network 2 //记录当前控制脉冲
LD SM0.0
MOVR LD8, AC0
-R VD4200, AC0
MOVR LD8, VD4200

Network 3 // 钢量转换把mm 转换成脉冲数量
LD SM0.0
MOVR AC0, LD12
/R 0.01, LD12
ROUND LD12, LD16
DTR LD16, AC0

Network 4
LDR< AC0, 0.0
RI Q0.2, 1
MOVR -1.0, AC2
*R AC0, AC2
NOT
MOVR AC0, AC2
SI Q0.2, 1

Network 5
LDR<= AC2, 0.0
CRET

Network 6
LD SM0.0
MOVB 3, VB4500

Network 7
LD SM0.0
MOVW -3, VW4503 //第一端的增量
MOVR 1.0, AC0
/R LD0, AC0
MOVR 1000000.0, AC3
*R AC0, AC3
ROUND AC3, AC3
DTI AC3, VW4501 //第一段的起始周期
MOVR 1.0, AC1
/R LD4, AC1
*R -1.0, AC1
+R AC0, AC1
/R 3E-006, AC1
ROUND AC1, VD4505 //第一段的脉冲数量

Network 8
LD SM0.0
MOVW +0, VW4511 //第二端的增量
MOVR 1.0, AC1
/R LD4, AC1
*R 1000000.0, AC1
ROUND AC1, AC1
DTI AC1, VW4509 //第二段的起始周期
ROUND AC2, AC0
-D VD4505, AC0
-D VD4505, AC0
MOVD AC0, VD4513 //第二段的脉冲数量

Network 9
LDD< VD4513, +0
MOVB 16#85, SMB67
MOVW +800, SMW68
ROUND AC2, AC1
MOVD AC1, SMD72
PLS 0
CRET

Network 10
LD SM0.0
MOVW +3, VW4519 //第三端的增量
MOVW VW4509, VW4517 //第三段的起始周期
MOVD VD4505, VD4521 //第三段的脉冲数量

Network 11 // 网络标题
// 网络注释
LD SM0.0
MOVB 16#A0, SMB67
MOVW +4500, SMW168
PLS 0
CRET


子程序stop_ch0:SBR31 //脉冲发生器复位PTO0
Network 1 // 网络标题
LD SM0.0
MOVB 0, SMB67
PLS 0
MOVR 0.0, VD4200
MOVR 0.0, VD3228
子程序 pls_ch1:SBR35 //控制脉冲发生器PTO1

Network 1
LDR= LD8, VD5200
CRET

Network 2
LD SM0.0
MOVR LD8, AC0
-R VD5200, AC0
MOVR LD8, VD5200

Network 3
// 钢量转换把mm 转换成脉冲数量
LD SM0.0
MOVR AC0, LD12
/R 0.01, LD12
ROUND LD12, LD16
DTR LD16, AC0

Network 4
LDR< AC0, 0.0
RI Q0.3, 1
MOVR -1.0, AC2
*R AC0, AC2
NOT
MOVR AC0, AC2
SI Q0.3, 1

Network 5
LDR<= AC2, 0.0
CRET

Network 6
LD SM0.0
MOVB 3, VB5500

Network 7
LD SM0.0
MOVW -3, VW5503 //第一端的增量
MOVR 1.0, AC0
/R LD0, AC0
MOVR 1000000.0, AC3
*R AC0, AC3
ROUND AC3, AC3
DTI AC3, VW5501 //第一段的起始周期
MOVR 1.0, AC1
/R LD4, AC1
*R -1.0, AC1
+R AC0, AC1
/R 3E-006, AC1
ROUND AC1, VD5505 //第一段的脉冲数量

Network 8
LD SM0.0
MOVW +0, VW5511 //第二端的增量
MOVR 1.0, AC1
/R LD4, AC1
*R 1000000.0, AC1
ROUND AC1, AC1
DTI AC1, VW5509 //第二段的起始周期
ROUND AC2, AC0
-D VD5505, AC0
-D VD5505, AC0
MOVD AC0, VD5513 //第二段的脉冲数量

Network 9
LDD< VD5513, +0
MOVB 16#85, SMB77
MOVW +800, SMW78
ROUND AC2, AC1
MOVD AC1, SMD82
PLS 1
CRET

Network 10
LD SM0.0
MOVW +3, VW5519 //第三端的增量
MOVW VW5509, VW5517 //第三段的起始周期
MOVD VD5505, VD5521 //第三段的脉冲数量

Network 11 // 网络标题
LD SM0.0
MOVB 16#A0, SMB77
MOVW +5500, SMW178
PLS 1
CRET


子程序stop_ch1:SBR36 //复位脉冲发生器PTO2
Network 1 // 网络标题
LD SM0.0
MOVB 0, SMB77
PLS 1
MOVR 0.0, VD5200
MOVR 0.0, VD5228