----------------机械手运动-------
--常用指令：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)            --获取当前位置
    
    P[1].x = -1000
    
    SetPointToMem(1,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))