众为兴SCARA四轴机械臂编程——基于ModbusTCP协议

Posted 2022-04-17 虎鲸不是鱼

众为兴SCARA四轴机械臂编程（一）——基于ModbusTCP协议

• 概述
• 方案原理
• 功能实现
• • 通讯功能
  • 机械臂编程
  • • 整体结构
    • 参考代码
• 要点
• 参考资料

概述

近年来工业机器人应用普遍，可以代替工厂普工完成简单的搬运、装配，分拣等工作。笔者给厂里的众为兴机械臂编写了该程序，用以完成取料、搬运、装配、抛废料等功能。

这种4轴机械臂大概是这样。。。笔者使用Solidworks2018 SP5建模，并使用Solidworks Visualize 2018开启GPU加速，渲染出图。

方案原理

西门子PLC S7-200SMART支持ModbusTCP协议，也可以接线控制机械臂。但ST20点位不足，ST60价格与S7-1200接近，性价比不高。买回来的机械臂用驱控一体机带动，支持IO接线、ModbusRTU（RS485接线）、ModbusTCP（占用502端口且只支持同网段）、标准TCP/IP 等多种通讯方式。故控制系统选用威纶通HMI 充当上位机，ST20充当中位机，驱控一体机充当下位机。
驱控一体机自带多个IO，可以直接带动24V电磁阀，自带双网口，支持5线程并发，从原理上讲，PLC完全是多余的，但由于种种不便公开的原因不得不用。

功能实现

通讯功能

选用ModbusTCP通讯，为了方便控制，使用单向方式，即PLC与驱控机的读写寄存器严格分开，不共用，防止出错。

机械臂编程

该机种采用ARStudio上位机软件编程，语法上与Lua脚本、嵌入式C语言很像，可以参照玩8051单片机、Arduino Mega2560的做法。

整体结构

定义变量，采用local关键字。
主函数function main中先执行初始化子函数init，再用while死循环重复执行Run子函数。
这种程序的本质是脚本，与人工按示教器的按钮来操机的做法、顺序都是一致的，纯面向过程。把子函数封装在run函数之后，可以调用子函数。

参考代码

--读取PLC输出状态(即机械臂输入状态)*********对应PLC地址VB100
local Command=publicread(0x00)
--[[
用该变量获取PLC输出状态
Command=0保留为空		Command=1回原点
Command=2去取料		Command=3去正扣位置
Command=4去反扣位置	Command=5去扣放
Command=6去正放位置	Command=7去反放位置
Command=8去NG抛料
]]--
--********************此处可定义其它动作**********************

--********************此处可定义其它动作**********************


local Flag_Get=publicread(0x02)		--传感器判断是否取到料
--[[
Flag_Get=0取料失败		Flag_Get=1取料成功

]]--

--写入PLC输入状态（即机械臂输出状态）*******对应PLC地址VB200
local Flag_Out=0
local Address_Out=0x04
--[[
用publicwrite(Address_Out,Flag_Out)改写状态
Flag_Out=0保留为空			Flag_Out=1正在回原点
Flag_Out=2回原点完成		    Flag_Out=3正在取料
Flag_Out=4取料成功且完成		Flag_Out=5正在去正扣位置
Flag_Out=6已到正扣位置		Flag_Out=7正在去反扣位置
Flag_Out=8已到反扣位置		Flag_Out=9正在扣盒
Flag_Out=10扣盒完成			Flag_Out=11取料失败且完成
Flag_Out=12正在去正放位置		Flag_Out=13已到正放位置
Flag_Out=14正在去反放位置		Flag_Out=15已到反放位置
Flag_Out=16正在去NG位置		Flag_Out=17已到NG位置
]]--
--********************此处可定义其它状态**********************

--********************此处可定义其它状态**********************



--*******************控制夹具***************************
local Flag_Tool=0
local Address_Tool=0x06
--[[
用publicwrite(Address_Tool,Flag_Tool)改写状态	
--Flag_Tool=0关闭夹具			Flag_Tool=1打开夹具
]]--

--*******************控制小气缸*************************
local Flag_Needle=0
local Address_Needle=0x08
--[[
用publicwrite(Address_Needle,Flag_Needle)改写状态	
--Flag_Needle=0关闭小气缸		Flag_Needle=1打开小气缸
]]--

--****************机器人点位***************************
local Pos_Ready=p1							--等待点（取料点正上方）
local Pos_Get=p2							--取料点
local Pos_Positive=p3						--正扣上方点
local Pos_Negative=p4						--反扣上方点
local Pos_Push_Positive=p5					--正扣点
local Pos_Push_Negative=p6					--反扣点
local Pos_Drop_Positive=p7					--正放点
local Pos_Drop_Negative=p8					--反放点
local Pos_NG=p9								--NG点
--*******************此处可定义其它点*******************

--*******************此处可定义其它点*******************


--***************定义变量****************
local Spd_Init = 20		    				--机器人初始化速率,范围 1~100（允许改动）
local Spd_Run = 20							--机器人正式运行速率,范围 1~100（允许改动）



--**************主程序入口***************
function main()
	Init()									--机器人初始化子函数
	--MotOn()									--机器人上电//调试用
	--Delay(500)								--延时500ms确保上电成功
	print("电机已使能")
	print("机器人进入复位状态")
	--MovP(p1)		--///调试用，调试完成后删除
	
	print("初始化或复位完成")
	--MovP(p2)		--///调试用，调试完成后删除
	while true do							--主程序需死循环方可连续运行，否则为单步运行
		Run()								--机器人运动控制子函数
	end
end

function Run()
	repeat									--防止跑飞
		Command=publicread(0x00)			--更新数值
		Delay(500)							--允许改小以提升响应速度
		print("Command=",Command)
	until(Command==1 or Command==2	or	Command==3	or	Command==4	or	Command==5	or	Command==6	or	Command==7	or	Command==8) 
	
	
	if ( Command == 1 ) then 				--Command=1，回原点
		print("接收到回原点信号，开始回原点")
		Action_Home()
		--MovP(p1)					--///调试用，调试完成后删除							
	end
	
	if ( Command == 2 ) then				--Command=2，去取料
		print("接收到取料信号，开始取料")
		Action_Get()
		--MovP(p2)					--///调试用，调试完成后删除
		--print("回p2完成")
		--Command=publicread(0x00)
	end
	
	if(  Command == 3 ) then				--Command=3,去正扣上方
		print("接收到去正扣上方信号，开始去正扣上方")
		MovP(p3)
		Action_Positive()
	end
	
	if( Command == 4 ) then					--Command=4,去反扣上方
		MovP(p4)
		print("接收到去反扣上方信号，开始去反扣上方")
		Action_Negative()
	end
	
	if( Command == 5 ) then					--Command=5.去扣盖
		print("接收到去扣盖信号，开始扣盖")
		Action_Push()
	end
	
	if( Command == 6 ) then					--Command=6,去正放位置
		print("接收到去正放信号，开始去正放")
		Action_Drop_Positive()
	end
	
	if( Command == 7 ) then					--Command=7,去反放位置
		print("接收到去反放信号，开始去反放")
		Action_Drop_Negative()
	end
	
	if( Command == 8 ) then					--Command==8,去NG位置
		print("接收到去NG抛料信号，开始去抛料")
		Action_NG()
	end
	
--********待补充的动作子函数置于此处即可调用*****开始区域

--********待补充的动作子函数置于此处即可调用*****结束区域
	
end

--********下方为子函数的具体代码*********************

function Init()								--初始化及复位至等待位置
	--MotOn()									--机器人上电
	--Delay(500)								--延时500ms确保上电成功
	print("电机已使能")
	print("机器人进入复位状态")
	sysrate(Spd_Init)						--机器人速度设定到初始化速度
	print("初始化运行速度为",Spd_Init,"%")
	publicwrite(Address_Out,1)
	MovJ(J3,Pos_Ready.z)					--将Z轴提升至和等待点相同的高度确保安全											
	print("Z轴已经移动到安全高度")
	if ( Flag_Get == 1 ) then				--判断机械手上是否有物料							
		print("机械手检测到有料")
		Action_Clear()						--有物料则执行清料子函数
	end
	MovP(Pos_Ready)							--机器人移动到等待点											
	print("抛料完成，已回到等待位置")
	publicwrite(Address_Out,2)				--复位完毕,改写输出状态												
	print("机械手移动到等待位置，准备完毕")
	sysrate(Spd_Run)						--机器人速度设定到运行速度
	print("运行速度已设定为",Spd_Run,"%")
end

function Action_Clear()						--清料子函数
	repeat
		Command=publicread(0x00)
		Delay(50)
		print("Command=",Command)
	until (Command==3 or Command==4 or Command==6 or Command==7	or	Command==8)
	if(  Command == 3 ) then				--Command=3,去正扣上方
		print("接收到去正扣上方信号，开始去正扣上方")
		Action_Positive()
		print("准备正扣料")
	end
	
	if( Command == 4 ) then					--Command=4,去反扣上方
		print("接收到去反扣上方信号，开始去反扣上方")
		Action_Negative()
		print("准备反扣料")
	end
	
	repeat
		Command=publicread(0x00)
		Delay(50)
		print("Command=",Command)
	until (Command == 5 or Command==6 or Command==7	or Command==8)
	if( Command == 5 ) then				--Command=5.去扣盒
		print("接收到去扣盒信号，开始扣盒")
		Action_Push()
	end
	
	repeat
		Command=publicread(0x00)
		Delay(50)
		print("Command=",Command)
	until	(Command==6 or Command==7	or Command==8)
	if( Command == 6 ) then					--Command=6,去正放位置
		print("接收到去正放信号，开始去正放")
		Action_Drop_Positive()
	end
	
	if( Command == 7 ) then					--Command=7,去反放位置
		print("接收到去反放信号，开始去反放")
		Action_Drop_Negative()
	end
	
	if( Command == 8 ) then					--Command=8,去NG抛料
		Action_NG()
		print("已NG抛料，机械手已清除携带的物料")
	end
	
end

function Action_Get()						--取料子函数
	publicwrite(Address_Out,3)
	MovP(Pos_Ready)
	Flag_Get=publicread(0x02)
	Delay(50)
	if(Flag_Get==0)	then
		print("传感器判断取料失败，开始取料")
		--Delay(5000)
		MovP(Pos_Get)
		waitrealpos()
		Action_ToolOpen()
		--publicwrite(Address_Needle,1)			--使用夹具
		Delay(500)
		--publicwrite(Address_Needle,0)
		MovP(Pos_Ready)							--上升后判断受否成功
		Flag_Get=publicread(0x02)
		Delay(500)
		if( Flag_Get == 0 ) then
			publicwrite(Address_Out,11)
			print("传感器判断取料失败，返回到原点")
			--Delay(5000)
			--Pause()
		else
			publicwrite(Address_Out,4)
			print("传感器判断取料成功，返回到原点")
			Delay(5000)
		end
	elseif(Flag_Get==1)then
		publicwrite(Address_Out,4)
		Delay(50)	
		print("已有料，无需取料")
	end
	
	
	
end

function Action_Positive()					--去正扣上方子函数
	publicwrite(Address_Out,5)
	MovP(Pos_Positive)
	waitrealpos()
	publicwrite(Address_Out,6)
	print("到达正扣上方")
end

function Action_Negative()					--去反扣上方子函数
	publicwrite(Address_Out,7)
	MovP(Pos_Negative)
	waitrealpos()
	publicwrite(Address_Out,8)
	print("到达反扣上方")
end

function Action_Push()						--压配子函数
	publicwrite(Address_Out,9)
	print("开始压配")
	MovJ(J3,Pos_Push_Positive.z)
	waitrealpos()
	Action_ToolClose()						--松开夹具
	publicwrite(Address_Needle,1)			--顶出小气缸
	Delay(500)
	publicwrite(Address_Needle,0)			--收回小气缸
	Delay(500)
	Action_ToolOpen()						--使用夹具
	Delay(500)
	MovJ(J3,Pos_Positive.z)
	publicwrite(Address_Out,10)
	print("已压配")
	--MovP(Pos_Ready)
	--waitrealpos()
	--print("已返回原点")
end

function Action_ToolOpen()					--打开夹具子函数
	publicwrite(Address_Tool,1)
	Delay(500)
end

function Action_ToolClose()					--关闭夹具子函数
	publicwrite(Address_Tool,0)
	Delay(500)
end

function Action_Drop_Positive()				--去正放子函数
	publicwrite(Address_Out,12)
	Delay(20)
	print("开始去正放点")
	MovP(Pos_Drop_Positive)
	publicwrite(Address_Out,13)
	print("已到达正放点")
	Action_ToolClose()
	Action_Home()
	--publicwrite(Address_Out,1)
	--MovP(Pos_Ready)
	--publicwrite(Address_Out,2)
end

function Action_Drop_Negative()				--去反放子函数
	publicwrite(Address_Out,14)
	Delay(20)
	print("开始去反放点")
	MovP(Pos_Drop_Negative)
	publicwrite(Address_Out,15)
	print("已到达反放点")
	Action_ToolClose()
	Action_Home()
	--publicwrite(Address_Out,1)
	--MovP(Pos_Ready)
	--publicwrite(Address_Out,2)
end

function Action_Home()						--回原点子函数
	publicwrite(Address_Out,1)
	Delay(20)
	--Init()
	MovP(Pos_Ready)					--///调试用，调试完成后删除
	waitrealpos()	
	print("回p1完成")
	publicwrite(Address_Out,2)
	Delay(100)
	--Command=publicread(0x00)	
end

function Action_NG()
	publicwrite(Address_Out,16)
	Delay(20)
	MovP(Pos_NG)
	waitrealpos()
	print("到NG点完成")
	publicwrite(Address_Out,17)
	Delay(20)
	Action_ToolClose()
	print("NG抛料完成，准备返回原点")
	Delay(500)
	Action_Home()
end

要点

点位需事先手动示教，防止撞机。
初始化及调机时需把速度调低，防止撞机及撞伤人员。
嵌入式开发与普通软件开发不同，需人为控制某些参数，如：等待时间，扫描频率。等待时间过短，会导致来不及从SRAM与DRAM中读取数据，或由于交换机等网络设备的延时读取数据失败进而卡死在某个环节。死循环中，扫描频率过高，会吃干CPU性能，造成程序跑飞、难以调试、通讯资源被挤占导致通讯中断等问题，难以保证程序的实时性，且工业生产中容易造成事故。很多参数都需要以严谨的态度小心调试，程序开发中也要注意逻辑的严密性。即使上位机、中位机、下位机都写了互锁与连锁，机械臂的程序中也尽量写上互锁，功能也要多写一些备用，不怕一万就怕万一，力求避免可能发生的各种意外。

参考资料

众为兴参考资料