众为兴SCARA四轴机械臂编程（一） |
您所在的位置：网站首页 › 工业机器人goto指令 › 众为兴SCARA四轴机械臂编程（一） |
众为兴SCARA四轴机械臂编程（一）
|
众为兴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性能，造成程序跑飞、难以调试、通讯资源被挤占导致通讯中断等问题，难以保证程序的实时性，且工业生产中容易造成事故。很多参数都需要以严谨的态度小心调试，程序开发中也要注意逻辑的严密性。即使上位机、中位机、下位机都写了互锁与连锁，机械臂的程序中也尽量写上互锁，功能也要多写一些备用，不怕一万就怕万一，力求避免可能发生的各种意外。 参考资料众为兴参考资料 |
【本文地址】