----------------机械手运动-------
--常用指令：MovP弧线运动、MovL直线运动
MovP(1) --弧线轨迹运动到一号点
MovP(2) --弧线轨迹运动到二号点
MovL(1) --直线轨迹运动到一号点
MovL(2) --直线轨迹运动到二号点
-----拱形运动
MovP(1 .. P.Z(-3000)) --弧线轨迹移动到1号点安全高度上方
MovP(1) --弧线轨迹移动到1号点
MovP(1 .. P.Z(-3000))
MovP(2 .. P.Z(-3000)) --弧线轨迹移动到2号点安全高度上方
MovP(2) --弧线轨迹移动到2号点
MovP(2 .. P.Z(-3000))
MovL(1 .. P.Z(-3000)) --直线轨迹移动到1号点安全高度上方
MovL(1) --直线轨迹移动到1号点
MovL(1 .. P.Z(-3000))
MovL(2 .. P.Z(-3000)) --直线轨迹移动到2号点安全高度上方
MovL(2) --直线轨迹移动到2号点
MovL(2 .. P.Z(-3000))
--拱形指令函数封装
--MovP拱形
function Arch_P(Point,Height) --Point：要移动的点位，Height：拱形最高安全高度
_,Here = motion.GetFeedbackPosition(1) --获取当前位置
MovP(Here .. P.Z(Height)) --当前位置抬高到安全高度
MovP(Point .. P.Z(Height)) --移动到目标点安全位置
MovP(Point) --移动到目标点
end
Arch_P(1,-3000) --拱形弧线移动到1号点，安全高度为-3000
Arch_P(2,-3000)
--MovL拱形
function Arch_L(Point,Height) --Point：要移动的点位，Height：拱形最高安全高度
_,Here = motion.GetFeedbackPosition(1) --获取当前位置
MovL(Here .. P.Z(Height)) --当前位置抬高到安全高度
MovL(Point .. P.Z(Height)) --移动到目标点安全位置
MovL(Point) --移动到目标点
end
Arch_L(1,-3000) --拱形直线移动到1号点，安全高度为-3000
Arch_L(2,-3000)
--动作轨迹优化，在运动指令中可以加PASS指令来平滑过渡运动轨迹，对于机械手运动效果和CT时间都有明显的提升，例如：
MovP(1 .. P.Z(-3000),"PASS") --弧线轨迹移动到1号点安全高度上方
MovP(1) --弧线轨迹移动到1号点
MovP(1 .. P.Z(-3000),"PASS")
MovP(2 .. P.Z(-3000),"PASS") --弧线轨迹移动到2号点安全高度上方
MovP(2) --弧线轨迹移动到2号点
MovP(2 .. P.Z(-3000),"PASS")
--注意： MovP与MovL之间是不同运动指令，不能加PASS，机械手左右手的时候，只能用MovP，不能用MovL
MovP(1 .. P.Z(-3000),"PASS") --弧线轨迹移动到1号点安全高度上方
MovP(1) --弧线轨迹移动到1号点
MovP(1 .. P.Z(-3000)) --MovP与MovL之间不能加PASS
MovL(2 .. P.Z(-3000),"PASS") --直线轨迹移动到2号点安全高度上方
MovL(2) --直线轨迹移动到2号点
MovL(2 .. P.Z(-3000),"PASS")
----------------机械手通讯-------
--常用通讯：IO通讯、TCP------MODBUS通讯、TCP------Socket(无协议)通讯、232/485通讯
--IO通讯，相关指令：User_DI()、User_DO
repeat until User_DI(1) == 1 --等待取料信号
MovP(1 .. P.Z(-3000))
MovP(1)
User_DO(1,1) --关闭夹爪
DELAY(0.2)
MovP(1 .. P.Z(-3000))
User_DO(2,1) --发送取料完成
repeat until User_DI(2) == 1 --等待放料信号
MovP(2 .. P.Z(-3000))
MovP(2)
User_DO(1,0) --打开夹爪
DELAY(0.2)
MovP(2 .. P.Z(-3000))
User_DO(3,1) --发送放料完成
--TCP------MODBUS通讯，相关指令：ModbusRead16()、ModbusWrite16()
repeat until ModbusRead16(0x2000) == 1 --等待取料信号
MovP(1 .. P.Z(-3000))
MovP(1)
User_DO(1,1) --关闭夹爪
DELAY(0.2)
MovP(1 .. P.Z(-3000))
ModbusWrite16(0x2001,1) --发送取料完成
repeat until ModbusRead16(0x2003) == 1 --等待放料信号
MovP(2 .. P.Z(-3000))
MovP(2)
User_DO(1,0) --打开夹爪
DELAY(0.2)
MovP(2 .. P.Z(-3000))
ModbusWrite16(0x2004,1) --发送放料完成
--TCP------Socket(无协议)通讯，相关指令：FreePort.ECM_OpenAsClient()、FreePort.ECM_Tx()、FreePort.ECM_Rx()、FreePort.ECM_CloseAll()、FreePort.ECM_ChkConnect()、FreePort.ECM_Clear()
function open_client_Vision() --连接视觉服务器 打开以太网作为客户端
repeat
FreePort.ECM_CloseAll() --关闭所有链接
FreePort.ECM_Clear(1) --清楚端口数据
DELAY(0.2)
open_result = FreePort.ECM_OpenAsClient(1,"192.168.1.11",6000,nil,nil) --识别号：1，链接ip：192.168.1.11，端口号：6000，开始符与结束符：nil
DELAY(0.2)
open_result = FreePort.ECM_ChkConnect(1) --检查链接状态
until open_result == 0
print("Link OK" .. "\n")
end
function Send_Out_Vision(value) --发送内容给视觉
repeat
send_result = FreePort.ECM_Tx(1,value) --发送value到识别号1的服务器
until send_result == 0
print("SendPC:" .. value .. '\n')
end
function Receive_Vision() --接收视觉发过来的内容
repeat
c_status_pc,return_text_pc,text_l_pc = FreePort.ECM_Rx(1) ---接收PC发送过来的分BIN结果
until return_text_pc ~= "" --CMD_L>0
print("ReceivePC:" .. return_text_pc .."\n") ---------------打印接收到的消息
return return_text_pc
end
open_client_Vision() --连接视觉服务器 打开以太网作为客户端
MovP(1 .. P.Z(-3000))
MovP(1)
User_DO(1,1) --关闭夹爪
DELAY(0.2)
MovP(1 .. P.Z(-3000))
::AA:: --NG跳转标签
Send_Out_Vision("A1") --发送拍照信号给视觉
Data1 = Receive_Vision() --接收视觉发过来的内容（带有分隔符字符串类型）
Data = string.split(Data1,",") --以指定的分隔符","来分割字符串，返回的是数组
if Data[1] == "NG" then
goto AA
end
X_Vision = Data[2]*1000 --X坐标
Y_Vision = Data[3]*1000 --Y坐标
C_Vision = Data[4]*1000 --C坐标
Point = P.new(X_Vision,Y_Vision,-10000,0,0,C_Vision,0,0,0,0,0,0,0) --创建新点位
MovP(Point .. P.Z(-3000))
MovP(Point)
User_DO(1,0) --打开夹爪
DELAY(0.2)
MovP(Point .. P.Z(-3000))