怎样去编写机械手臂的程序？有大神遇到过这个问题吗

　　下一步进行机械手臂的程序编写
　　程序只是进行简单的点位运动，实现抓取功能
　　程序控制的点位表
　　［tr］输入点位点位描述输出点位点位描述［/tr］DI5夹取完成DO5夹取物料
　　DI6放料完成DO6放下物料
　　DI7回原点DO7设备就绪
　　DI8物料到位
　　程序如下
　　MODULE MainModule CONST robtarget phome：=［［364.35，0.00，594.00］，［0.5，0，0.866025，0］，［0，0，0，0］，［9E+09，9E+09，9E+09，9E+09，9E+09，9E+09］］; CONST robtarget ppick：=［［364.35，0.00，594.00］，［0.5，0，0.866025，0］，［0，0，0，0］，［9E+09，9E+09，9E+09，9E+09，9E+09，9E+09］］; CONST robtarget pplace：=［［364.35，0.00，594.00］，［0.5，0，0.866025，0］，［0，0，0，0］，［9E+09，9E+09，9E+09，9E+09，9E+09，9E+09］］; CONST robtarget pq1：=［［364.35，0.00，594.00］，［0.5，0，0.866025，0］，［0，0，0，0］，［9E+09，9E+09，9E+09，9E+09，9E+09，9E+09］］; CONST robtarget pq2：=［［364.35，0.00，594.00］，［0.5，0，0.866025，0］，［0，0，0，0］，［9E+09，9E+09，9E+09，9E+09，9E+09，9E+09］］; VAR num num1：=0; PROC main（） gohome; //1 WHILE TRUE DO IF di7 = 1 THEN //2 gohome; ENDIF IF di8 = 1 THEN //3 Reset do7; pick; ENDIF ENDWHILE ENDPROC PROC pick（） MoveJ pq1，v200，fine，tool0; MoveJ Offs（ppick，0，0，100），v100，fine，tool0; MoveL ppick，v100，fine，tool0; Set do5; WaitTime 1; //4 WaitDI di5，1; //5 Reset do5; Movel Offs（ppick，0，0，100），v100，fine，tool0; MoveJ pq1，v200，fine，tool0; place; endproc PROC place（） Movej pq2，v200，fine，tool0; MoveJ Offs（pplace，0，0，100），v200，fine，tool0; MoveL pplace，v100，fine，tool0; Set do6; WaitTime 1; WaitDI di6，1; Reset do6; MoveL Offs（pplace，0，0，100），v100，fine，tool0; MoveJ pq2，v200，fine，tool0; IF di8 = 1 THEN //6 pick; ENDIF MoveJ phome，v200，fine，tool0; //7 endproc PROC gohome（） //8 MoveJ phome，v200，fine，tool0; reset do5； reset do6； reset do7； Set do7; ENDPROC ENDMODULE 注释
　　1、上电程序运行后，执行回原点程序，进行程序的初始化；
　　2、控制器在程序运行中控制机器人进行回原点及初始化。
　　3、物料到位后，plc控制器给予信号，让机械手臂，执行抓取物料程序（pick）
　　4、在抓取物料程序中，先关节运动至过度点pq1.在关节运动至抓取点上方100mm处，直线运动至抓取点（ppick），输出夹取信号，控制器控制气缸夹具对物料进行夹取，延时一秒。
　　5、等待信号，等待控制器将物料夹取完成。
　　6、在放物料程序中，上方是与取物料同样的控制逻辑，这里到达放物料的pq2过度点后，对于物料是否到位进行了判断，如果物料到位这直接执行取物料程序，直接动作至取物料的pq1点位，以节省时间，优化动作。
　　7、放物料完成后，且并没有新的物料到位，则回原点，返回主程序，等待新物料到位后执行取物料程序。
　　8、回原点及初始化程序。