[機器手臂程式設計實作] 3維陣列演練 : 模擬擺放 2 個 3*3 矩陣

<source : unflash>

一、情境描述

模擬讓機器手臂置物，並在2個呈現階梯狀的 3*3 矩陣位置，放置物品。

動作流程，依序為 :

1. 前往初始化原點。
2. 移動到第一個 3*3 矩陣置放點上方。
3. 依據指定的順序先完成第一個 3*3 矩陣的擺放
4. 移動到第二個 3*3 矩陣，依指定的順序擺放
5. 回到指定的原點。

示意圖如下 :

<製圖 : great way>

二、程式筆記

(一)、完整範例程式

備註 : 本程式以 HIWIN IDE為實作IDE

(二)、程式解說

1.E6POINT pint

宣告 pint 變數，以各軸角度或XYZ座標方式描述位置。

2.INT A, B, C

宣告 3 個整數變數，分別為A、B、C，作為迴圈次數的設定。

A為執行X軸的動作。

B為執行Y的動作。

C為執行兩個矩陣間高度的動作。同時也是矩陣的單位數，本例 C = 0 TO 1，總計完成 2 個 矩陣。

3.$DO[1] = TRUE

數位輸出狀態設定為ON，此處表示 "打開夾爪 "。

4.PTP P1 CONT Vel=100% Acc=50% TOOL[0] BASE[0]

以 PTP (點到點)的方式移動到 P1。

5.以巢狀迴圈的方式來設定擺放動作之架構

FOR C = 0 TO 1

    FOR B = 0 TO 2

        FOR A = 0 TO 2

...

        ENDFOR

    ENDFOR

ENDFOR

A為執行X軸的動作。

B為執行Y的動作。

C為執行兩個矩陣間高度的動作。同時也是矩陣的單位數，本例 C = 0 TO 1，總計完成 2 個 矩陣。

6.設定擺放動作

pint = P1 // 將 P1的位置值給 pint 變數

pint.X = pint.X + 50*A // X軸的動作為 pint.X + 50公分乘A迴圈當下計次

pint.Y = pint.Y + 50*B + 150*C // Y軸的動作為 pint.Y + 50公分乘B迴圈當下計次 + 150公分乘C迴圈當下計次

pint.Z = pint.Z + 100*C // Z軸的動作為 pint.Z + 100公分乘C迴圈當下計次

LIN pint CONT Vel=2000mm/s Acc=50% TOOL[0] BASE[0] //以 Line的方式移動到 pint 點

以下程式碼為描述夾爪移動與夾取的行為 :

LIN_REL{Z -150} //  往下移動 150 公分

$DO[1] = FALSE //夾爪閉合

WAIT SEC 0.5 //等待0.5秒

$DO[1] = TRUE //夾爪打開

WAIT SEC 0.5  //等待0.5秒

LIN_REL{Z 150} //往上移動 150 公分

7.PTP P0 CONT Vel=100% Acc=50% TOOL[0] BASE[0]

回到指定的原點。

---

參考資料 :

機器人系統軟體 - 上銀科技