DOBOTに緊急停止スイッチを取り付ける

やりたいこと

DOBOT Magicianには、緊急停止用のボタンが搭載されておらず、また、ハードウェア的に緊急停止する機能が実装できるように設計されていないようです。

ただ、DOBOTの動作を止める（緊急停止）APIは用意されているので、そちらと外部スイッチを用意して、ソフトウェア的に簡易な緊急停止スイッチを作りたいと思います。プログラム上でスイッチの入力を監視し、入力があれば動作を停止する命令を送るものなので、いわゆる緊急停止スイッチのようにどのような状況でも安全に停止できることを保証していない点、ご留意ください。

できたもの

スイッチの接続

今回のプログラムでは、DOBOT背面のEIO14とEIO20を使用しています。
接続は、以下の画像を参照してください。

動作

下記に示しているプログラムを3回動作させ、
1回目では、アラーム検知→動作再開を確認、
2回目では、緊急停止→プログラムの停止を確認、
3回目では、緊急停止→動作再開を確認しています

プログラム

テキストで使用しているdType.SetPTPCmdEx()は、指定座標への移動が完了するまで待ってしまい、アラームやEIOの確認ができません。
そのため、以下のプログラムでは、dType.SetPTPCmd()で指定座標への移動キューを追加し、動作が完了するか、アラームを検知するか、EIO14が入力されるまで確認し続けるmove()を作成しています。

emergency_stop.py

import DobotDllType as dType
from DobotDllType import PTPMode, JC, DobotConnect

def move(api, mode, x, y, z, r):
    ret = dType.SetPTPCmd(api, mode, x, y, z, r, True)
    cur_idx = dType.GetQueuedCmdCurrentIndex(api)
    print("start -- index [", cur_idx[0], "/", ret[0], "]")

    while True:
        cur_idx = dType.GetQueuedCmdCurrentIndex(api)
        alarm, len = dType.GetAlarmsState(api)
        alarm = int.from_bytes(alarm, 'little')

        if alarm != 0: # アラーム検知
            print("alarm")
            dType.dSleep(1000) # 1000msec待機
            dType.ClearAllAlarmsState(api) # アラームを解除
            print("alarm clear")
            break

        if ret[0] <= cur_idx[0]: # 動作が完了
            break

        if dType.GetIODI(api, 14)[0] == 0: # 外部スイッチ(EIO14)を押下 → 動作停止
            dType.SetQueuedCmdForceStopExec(api)
            print("stop")
            while dType.GetIODI(api, 14)[0] == 0: # スイッチが離れるまで待つ
                pass

            while True:
                if dType.GetIODI(api, 14)[0] == 0: # 外部スイッチ(EIO14)を押下 → プログラム終了
                    print("emergency stop")
                    while dType.GetIODI(api, 14)[0] == 0: # スイッチが離れるまで待つ
                        pass
                    dType.SetQueuedCmdStopExec(api) # 動作停止
                    dType.DisconnectDobot(api) # DOBOT接続解除
                    exit()

                if dType.GetIODI(api, 20)[0] == 0: # 外部スイッチ(EIO20)を押下 → 動作再開（次のキューから）
                    while dType.GetIODI(api, 20)[0] == 0: # スイッチが離れるまで待つ
                        pass
                    dType.SetQueuedCmdStartExec(api) # 動作再開
                    print("restart")
                    break
            break
        dType.dSleep(5)
    print("end   -- index [", cur_idx[0], "/", ret[0], "]")

api = dType.load()
ret, fwtype, version = dType.ConnectDobot(api, "", 115200)
if not ret == DobotConnect.DobotConnect_NoError:
    print("Could not connect to DOBOT")
    exit()

# 初期設定
dType.SetCmdTimeout(api, 3000)
dType.SetQueuedCmdClear(api)
dType.SetQueuedCmdStartExec(api)

deviceName = "DOBOT Magician"
dType.SetDeviceName(api, deviceName)
dType.SetJOGJointParams(api, 50, 50, 50, 50, 50, 50, 50, 50, True)
dType.SetJOGCoordinateParams(api, 50, 50, 50, 50, 50, 50, 50, 50, True)
dType.SetJOGCommonParams(api, 100, 100, True)
dType.SetPTPJointParams(api, 200, 200, 200, 200, 200, 200, 200, 200, True)
dType.SetPTPCoordinateParams(api, 200, 200, 200, 200, True)
dType.SetPTPJumpParams(api, 20, 100, True)
dType.SetPTPCommonParams(api, 30, 30, True)
dType.SetHOMEParams(api, 200, 0, 0, 0, True)
dType.SetEndEffectorParams(api, 59.7 ,0, 0, 0)

# EIOの設定
dType.SetIOMultiplexingEx(api, 14, 3, True) # 外部スイッチ EIO14
dType.SetIOMultiplexingEx(api, 20, 3, True) # 外部スイッチ EIO20


# ホーム動作
# dType.SetHOMECmdEx(api, 0, True)

move(api, PTPMode.PTPMOVLXYZMode, 200, -50, 0, 0)
move(api, PTPMode.PTPMOVLXYZMode, 350, 0, 0, 0) # 可動域外に移動
move(api, PTPMode.PTPMOVLXYZMode, 200, 0, 0, 0)

# DOBOT接続解除
dType.SetQueuedCmdStopExec(api)
dType.DisconnectDobot(api)