LoginSignup
0
1

More than 1 year has passed since last update.

@shimamotosan(Nao SHIMAMOTO)in株式会社アフレル

1台のPCから2台のDOBOTを切り替えながら制御する

Posted at

やりたいこと

1台のPCから2台のDOBOT Magicianを制御したいと思います。
DobotStudioでは、プログラムを実行しながら接続するDOBOTを切り替えられないので、PythonやC言語等、各種プログラミング言語を使用します。

公式に提供されているAPIには、DOBOTを複数台同時に接続する方法がありませんので、今回は都度接続するDOBOTを切り替えながら制御したいと思います。

動作

  • 1台目のDOBOTでペンを掴み、2台目のDOBOTに渡せる位置に移動します。
  • 2台目のDOBOTに切り替え、ペンを掴みに行きます。
  • 1台目のDOBOTに切り替え、ペンを離して最初の位置に戻ります。
  • 2代目のDOBOTに切り替え、ペンを置いて、最初の位置に戻ります。

動画では少し確認しづらいですが、ペンは拾い上げて浮かせた位置で受け渡しています。
1台目のDOBOTから2台目のDOBOTへ接続を切り替えても、1台目のDOBOTはアームやグリッパー（空気ポンプ）の状態を維持したままなので、ペンを落とすことなく受け渡すことが出来ます。

サンプルプログラム

2台のDOBOTを切り替えるためにそれぞれのCOMポートを指定しています。
お使いの環境に合わせて修正してください。

Python

two_dobot_sample.py
import DobotDllType as dType
from DobotDllType import PTPMode, JC, DobotConnect

# APIをロードし、DOBOTに接続する
api = dType.load()

def dobot_connect(com):
    state = dType.ConnectDobot(api, com, 115200)[0]
    if not state == DobotConnect.DobotConnect_NoError:
        print("Could not connect to DOBOT")
        exit()

    # 初期設定
    dType.SetCmdTimeout(api, 3000)
    dType.SetQueuedCmdClear(api)
    dType.SetQueuedCmdStartExec(api)
    deviceName = "DOBOT Magician " + com
    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)

    print("Connect --> ", dType.GetDeviceName(api))

dobot_connect("COM3")
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, 0, 0, 0, True)
dType.SetEndEffectorGripperEx(api, True, False, True)
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, 0, -40, 0, True)
dType.SetEndEffectorGripperEx(api, True, True, True)
dType.dSleep(500)
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, 100, 0, 0, True)
dType.DisconnectDobot(api)

dobot_connect("COM4")
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, 0, 50, 0, True)
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, -100, 50, 0, True)
dType.SetEndEffectorGripperEx(api, True, False, True)
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, -100, 0, 0, True)
dType.SetEndEffectorGripperEx(api, True, True, True)
dType.DisconnectDobot(api)

dobot_connect("COM3")
dType.SetEndEffectorGripperEx(api, True, False, True)
dType.dSleep(500)
dType.SetEndEffectorGripperEx(api, False, False, True)
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, 0, 0, 0, True)
dType.DisconnectDobot(api)

dobot_connect("COM4")
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, 100, 0, 0, True)
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, 100, -35, 0, True)
dType.SetEndEffectorGripperEx(api, True, False, True)
dType.dSleep(500)
dType.SetEndEffectorGripperEx(api, False, False, True)
dType.SetPTPCmdEx(api, PTPMode.PTPMOVLXYZMode, 200, 0, 0, 0, True)
dType.DisconnectDobot(api)

C言語（C++）

two_dobot_sample.cpp
#include "stdafx.h"
#include <stdio.h>
#include <memory.h>
#include <windows.h>
#include "../DobotDll/DobotDll.h"

int dobot_connect(char* com)
{
    // Dobotに接続する
    char fwType[60];
    char version[60];
    float time[60];
    int ret = ConnectDobot(com, 115200, fwType, version, time);
    if (ret != DobotConnect_NoError) {
        printf("Could not connect to DOBOT.\n");
        return 0;
    }

    // 初期設定
    SetCmdTimeout(3000);

    const char deviceName[] = "Dobot Magician";
    SetDeviceName(deviceName);

    char deviceSN[64];
    GetDeviceName(deviceSN, sizeof(deviceSN));

    uint64_t cmdIndex = 0;

    JOGJointParams jsParam;
    for (int i = 0; i < 4; i++) {
        jsParam.velocity[i] = 50;
        jsParam.acceleration[i] = 50;
    }
    SetJOGJointParams(&jsParam, false, &cmdIndex);

    JOGCoordinateParams jcParam;
    for (int i = 0; i < 4; i++) {
        jcParam.velocity[i] = 50;
        jcParam.acceleration[i] = 50;
    }
    SetJOGCoordinateParams(&jcParam, false, &cmdIndex);

    JOGCommonParams jdParam;
    jdParam.velocityRatio = 100;
    jdParam.accelerationRatio = 100;
    SetJOGCommonParams(&jdParam, false, &cmdIndex);

    PTPJointParams pbsParam;
    for (int i = 0; i < 4; i++) {
        pbsParam.velocity[i] = 200;
        pbsParam.acceleration[i] = 200;
    }
    SetPTPJointParams(&pbsParam, false, &cmdIndex);

    PTPCoordinateParams cpbsParam;
    cpbsParam.xyzVelocity = 200;
    cpbsParam.xyzAcceleration = 200;
    cpbsParam.rVelocity = 200;
    cpbsParam.rAcceleration = 200;
    SetPTPCoordinateParams(&cpbsParam, false, &cmdIndex);

    PTPJumpParams pjp;
    pjp.jumpHeight = 20;
    pjp.zLimit = 100;
    SetPTPJumpParams(&pjp, false, &cmdIndex);

    PTPCommonParams pbdParam;
    pbdParam.velocityRatio = 30;
    pbdParam.accelerationRatio = 30;
    SetPTPCommonParams(&pbdParam, false, &cmdIndex);

    HOMEParams homeParam;
    homeParam.x = 200;
    homeParam.y = 0;
    homeParam.z = 0;
    homeParam.r = 0;
    SetHOMEParams(&homeParam, false, &cmdIndex);

    EndEffectorParams endeffectorParams;
    endeffectorParams.xBias = 59.7;
    endeffectorParams.yBias = 0;
    endeffectorParams.zBias = 0;
    SetEndEffectorParams(&endeffectorParams, false, &cmdIndex);

    printf("Connect -> %s\n", com);
}

void move(uint64_t CommandIndex, uint64_t cmdIdx, PTPCmd ptpCmd, int x, int y, int z, int r) {
    ptpCmd.ptpMode = PTPMOVLXYZMode;
    ptpCmd.x = x;
    ptpCmd.y = y;
    ptpCmd.z = z;
    ptpCmd.r = r;
    SetPTPCmd(&ptpCmd, true, &CommandIndex);
    do {
        cmdIdx = 0;
        if (GetQueuedCmdCurrentIndex(&cmdIdx) != 0) {
            Sleep(100);
        }
    } while (cmdIdx < CommandIndex);
}

int main(int argc, char* argv[])
{
    // コマンド構造体の変数宣言
    PTPCmd ptpCmd;
    memset(&ptpCmd, 0, sizeof(ptpCmd));

    // キューコマンドの最後の番号
    uint64_t CommandIndex = 0;
    uint64_t cmdIdx = 0;

    char com_port_1[] = "COM3";
    char com_port_2[] = "COM4";

    dobot_connect(com_port_1);
    move(CommandIndex, cmdIdx, ptpCmd, 200, 0, 0, 0);
    SetEndEffectorGripper(true, false, true, &CommandIndex);
    move(CommandIndex, cmdIdx, ptpCmd, 200, 0, -40, 0);
    SetEndEffectorGripper(true, true, true, &CommandIndex);
    Sleep(500);
    move(CommandIndex, cmdIdx, ptpCmd, 200, 100, 0, 0);
    DisconnectDobot();


    dobot_connect(com_port_2);
    move(CommandIndex, cmdIdx, ptpCmd, 200, 0, 50, 0);
    move(CommandIndex, cmdIdx, ptpCmd, 200, -100, 50, 0);
    SetEndEffectorGripper(true, false, true, &CommandIndex);
    move(CommandIndex, cmdIdx, ptpCmd, 200, -100, 0, 0);
    SetEndEffectorGripper(true, true, true, &CommandIndex);
    DisconnectDobot();


    dobot_connect(com_port_1);
    SetEndEffectorGripper(true, false, true, &CommandIndex);
    Sleep(500);
    SetEndEffectorGripper(false, false, true, &CommandIndex);
    move(CommandIndex, cmdIdx, ptpCmd, 200, 0, 0, 0);
    DisconnectDobot();


    dobot_connect(com_port_2);
    move(CommandIndex, cmdIdx, ptpCmd, 200, 100, 0, 0);
    move(CommandIndex, cmdIdx, ptpCmd, 200, 100, -35, 0);
    SetEndEffectorGripper(true, false, true, &CommandIndex);
    Sleep(500);
    SetEndEffectorGripper(false, false, true, &CommandIndex);
    move(CommandIndex, cmdIdx, ptpCmd, 200, 0, 0, 0);
    DisconnectDobot();

    return 0;
}

C#

Program.cs
using System;
using System.Text;
using DobotClientDemo.CPlusDll;

namespace sample00
{
    class Program
    {
        static void dobot_connect(string com)
        {
            // DOBOTを接続する
            StringBuilder fwType = new StringBuilder(60);
            StringBuilder version = new StringBuilder(60);
            int ret = DobotDll.ConnectDobot(com, 115200, fwType, version);
            if (ret != (int)DobotConnect.DobotConnect_NoError)
            {
                Console.WriteLine("Could not connect to DOBOT.");
                return;
            }

            // 初期設定
            DobotDll.SetCmdTimeout(3000);

            string deviceName = "Dobot Magician";
            DobotDll.SetDeviceName(deviceName);

            StringBuilder deviceSN = new StringBuilder(64);
            DobotDll.GetDeviceName(deviceSN, 64);

            UInt64 cmdIndex = 0;

            JOGJointParams jsParam;
            jsParam.velocity = new float[] { 50, 50, 50, 50 };
            jsParam.acceleration = new float[] { 50, 50, 50, 50 };
            DobotDll.SetJOGJointParams(ref jsParam, false, ref cmdIndex);

            JOGJointParams jcParam;
            jcParam.velocity = new float[] { 50, 50, 50, 50 };
            jcParam.acceleration = new float[] { 50, 50, 50, 50 };
            DobotDll.SetJOGJointParams(ref jcParam, false, ref cmdIndex);

            JOGCommonParams jdParam;
            jdParam.velocityRatio = 100;
            jdParam.accelerationRatio = 100;
            DobotDll.SetJOGCommonParams(ref jdParam, false, ref cmdIndex);

            PTPJointParams pbsParam;
            pbsParam.velocity = new float[] { 200, 200, 200, 200 };
            pbsParam.acceleration = new float[] { 200, 200, 200, 200 };
            DobotDll.SetPTPJointParams(ref pbsParam, false, ref cmdIndex);

            PTPCoordinateParams cpbsParam;
            cpbsParam.xyzVelocity = 200;
            cpbsParam.xyzAcceleration = 200;
            cpbsParam.rVelocity = 200;
            cpbsParam.rAcceleration = 200;
            DobotDll.SetPTPCoordinateParams(ref cpbsParam, false, ref cmdIndex);

            PTPJumpParams pjp;
            pjp.jumpHeight = 20;
            pjp.zLimit = 100;
            DobotDll.SetPTPJumpParams(ref pjp, false, ref cmdIndex);

            PTPCommonParams pbdParam;
            pbdParam.velocityRatio = 30;
            pbdParam.accelerationRatio = 30;
            DobotDll.SetPTPCommonParams(ref pbdParam, false, ref cmdIndex);

            HOMEParams homeParam;
            homeParam.x = 200;
            homeParam.y = 0;
            homeParam.z = 0;
            homeParam.r = 0;
            DobotDll.SetHOMEParams(ref homeParam, false, ref cmdIndex);

            Console.WriteLine("Connect -> {0}", com);
        }

        static void move(PTPCmd ptpCmd, ulong CommandIndex, ulong cmdIdx, int x, int y, int z, int r)
        {
            ptpCmd.ptpMode = (byte)PTPMode.PTPMOVLXYZMode;
            ptpCmd.x = x;
            ptpCmd.y = y;
            ptpCmd.z = z;
            ptpCmd.rHead = r;
            DobotDll.SetPTPCmd(ref ptpCmd, true, ref CommandIndex);
            do
            {
                cmdIdx = 0;
                if (DobotDll.GetQueuedCmdCurrentIndex(ref cmdIdx) != 0) System.Threading.Thread.Sleep(100);
            } while (cmdIdx < CommandIndex);
        }


        static void Main(string[] args)
        {
            //インスタンス生成
            PTPCmd ptpCmd = new PTPCmd();
            // キューコマンドの最後の番号
            ulong CommandIndex = 0;
            ulong cmdIdx = 0;


            dobot_connect("COM3");
            move(ptpCmd, CommandIndex, cmdIdx, 200, 0, 0, 0);
            DobotDll.SetEndEffectorGripper(true, false, true, ref CommandIndex);
            move(ptpCmd, CommandIndex, cmdIdx, 200, 0, -40, 0);
            DobotDll.SetEndEffectorGripper(true, true, true, ref CommandIndex);
            System.Threading.Thread.Sleep(500);
            move(ptpCmd, CommandIndex, cmdIdx, 200, 100, 0, 0);
            DobotDll.DisconnectDobot();


            dobot_connect("COM4");
            move(ptpCmd, CommandIndex, cmdIdx, 200, 0, 50, 0);
            move(ptpCmd, CommandIndex, cmdIdx, 200, -100, 50, 0);
            DobotDll.SetEndEffectorGripper(true, false, true, ref CommandIndex);
            move(ptpCmd, CommandIndex, cmdIdx, 200, -100, 0, 0);
            DobotDll.SetEndEffectorGripper(true, true, true, ref CommandIndex);
            DobotDll.DisconnectDobot();


            dobot_connect("COM3");
            DobotDll.SetEndEffectorGripper(true, false, true, ref CommandIndex);
            System.Threading.Thread.Sleep(500);
            DobotDll.SetEndEffectorGripper(false, false, true, ref CommandIndex);
            move(ptpCmd, CommandIndex, cmdIdx, 200, 0, 0, 0);
            DobotDll.DisconnectDobot();


            dobot_connect("COM4");
            move(ptpCmd, CommandIndex, cmdIdx, 200, 100, 0, 0);
            move(ptpCmd, CommandIndex, cmdIdx, 200, 100, -35, 0);
            DobotDll.SetEndEffectorGripper(true, false, true, ref CommandIndex);
            System.Threading.Thread.Sleep(500);
            DobotDll.SetEndEffectorGripper(false, false, true, ref CommandIndex);
            move(ptpCmd, CommandIndex, cmdIdx, 200, 0, 0, 0);
            DobotDll.DisconnectDobot();
        }
    }
}

Java

Sample00.java
package sample;

import com.sun.jna.Memory;
import com.sun.jna.Pointer;
import com.sun.jna.ptr.IntByReference;

import CPlusDll.DobotDll;
import CPlusDll.DobotDll.DobotConnectResult;
import CPlusDll.DobotDll.HOMEParams;
import CPlusDll.DobotDll.JOGCommonParams;
import CPlusDll.DobotDll.JOGCoordinateParams;
import CPlusDll.DobotDll.JOGJointParams;
import CPlusDll.DobotDll.PTPCmd;
import CPlusDll.DobotDll.PTPCommonParams;
import CPlusDll.DobotDll.PTPCoordinateParams;
import CPlusDll.DobotDll.PTPJointParams;
import CPlusDll.DobotDll.PTPJumpParams;
import CPlusDll.DobotDll.PTPMode;

public class Sample00 {

    public static void dobot_connect(String com) {
        Pointer portName = new Memory(60);
        portName.setString(0, com);
        Pointer fwType = new Memory(60);
        Pointer version = new Memory(60);
        int ret = DobotDll.instance.ConnectDobot(portName, 115200, fwType, version);
        if (DobotConnectResult.NoError.equals(ret) != true) {
            Msg("Could not connect to DOBOT.");
            return;
        }

        DobotDll.instance.SetCmdTimeout(3000);
        DobotDll.instance.SetQueuedCmdClear();
        DobotDll.instance.SetQueuedCmdStartExec();

        Pointer deviceName = new Memory(64);
        deviceName.setString(0, "Dobot Magician");
        DobotDll.instance.SetDeviceName(deviceName);

        Pointer deviceSN = new Memory(64);
        DobotDll.instance.GetDeviceName(deviceSN, 64);

        IntByReference cmdIndex = new IntByReference();

        JOGJointParams jsParam = new JOGJointParams();
        jsParam.velocity = new float[] { 50, 50, 50, 50 };
        jsParam.acceleration = new float[] { 50, 50, 50, 50 };
        DobotDll.instance.SetJOGJointParams(jsParam, false, cmdIndex);

        JOGCoordinateParams jcParam = new JOGCoordinateParams();
        jcParam.velocity = new float[] { 50, 50, 50, 50 };
        jcParam.acceleration = new float[] { 50, 50, 50, 50 };
        DobotDll.instance.SetJOGCoordinateParams(jcParam, false, cmdIndex);

        JOGCommonParams jdParam = new JOGCommonParams();
        jdParam.velocityRatio = 100;
        jdParam.accelerationRatio = 100;
        DobotDll.instance.SetJOGCommonParams(jdParam, false, cmdIndex);

        PTPJointParams pbsParam = new PTPJointParams();
        pbsParam.velocity = new float[] { 100, 100, 100, 100 };
        pbsParam.acceleration = new float[] { 100, 100, 100, 100 };
        DobotDll.instance.SetPTPJointParams(pbsParam, false, cmdIndex);

        PTPCoordinateParams cpbsParam = new PTPCoordinateParams();
        cpbsParam.xyzVelocity = 100;
        cpbsParam.xyzAcceleration = 100;
        cpbsParam.rVelocity = 100;
        cpbsParam.rAcceleration = 100;
        DobotDll.instance.SetPTPCoordinateParams(cpbsParam, false, cmdIndex);

        PTPJumpParams pjp = new PTPJumpParams();
        pjp.jumpHeight = 20;
        pjp.zLimit = 100;
        DobotDll.instance.SetPTPJumpParams(pjp, false, cmdIndex);

        PTPCommonParams pbdParam = new PTPCommonParams();
        pbdParam.velocityRatio = 30;
        pbdParam.accelerationRatio = 30;
        DobotDll.instance.SetPTPCommonParams(pbdParam, false, cmdIndex);

        HOMEParams homeParam = new HOMEParams();
        homeParam.x = 200;
        homeParam.y = 0;
        homeParam.z = 0;
        homeParam.r = 0;
        DobotDll.instance.SetHOMEParams(homeParam, false, cmdIndex);

        Msg("Coneect -> " + com);
    }

    public static void move(PTPCmd ptpCmd, IntByReference commandIndex, IntByReference cmdIdx, int x, int y, int z, int r) {
        ptpCmd.ptpMode = PTPMode.PTPMOVLXYZMode.getByte();
        ptpCmd.x = x;
        ptpCmd.y = y;
        ptpCmd.z = z;
        ptpCmd.r = r;
        DobotDll.instance.SetPTPCmd(ptpCmd, true, commandIndex);
        do {
            cmdIdx.setValue(0);
            if (DobotDll.instance.GetQueuedCmdCurrentIndex(cmdIdx) != 0) {}
        } while (cmdIdx.getValue() < commandIndex.getValue());
    }

    public static void main(String[] args) throws InterruptedException {

        // インスタンスを生成
        PTPCmd ptpCmd = new PTPCmd();

        // キューコマンドの最後の番号
        IntByReference commandIndex = new IntByReference();
        IntByReference cmdIdx = new IntByReference();


        dobot_connect("COM3");
        move(ptpCmd, commandIndex, cmdIdx, 200, 0, 0, 0);
        DobotDll.instance.SetEndEffectorGripper(true, false, true, commandIndex);
        move(ptpCmd, commandIndex, cmdIdx, 200, 0, -40, 0);
        DobotDll.instance.SetEndEffectorGripper(true, true, true, commandIndex);
        Thread.sleep(500);
        move(ptpCmd, commandIndex, cmdIdx, 200, 100, 0, 0);
        DobotDll.instance.DisconnectDobot();


        dobot_connect("COM4");
        move(ptpCmd, commandIndex, cmdIdx, 200, 0, 50, 0);
        move(ptpCmd, commandIndex, cmdIdx, 200, -100, 50, 0);
        DobotDll.instance.SetEndEffectorGripper(true, false, true, commandIndex);
        move(ptpCmd, commandIndex, cmdIdx, 200, -100, 0, 0);
        DobotDll.instance.SetEndEffectorGripper(true, true, true, commandIndex);
        DobotDll.instance.DisconnectDobot();


        dobot_connect("COM3");
        DobotDll.instance.SetEndEffectorGripper(true, false, true, commandIndex);
        Thread.sleep(500);
        DobotDll.instance.SetEndEffectorGripper(false, false, true, commandIndex);
        move(ptpCmd, commandIndex, cmdIdx, 200, 0, 0, 0);
        DobotDll.instance.DisconnectDobot();


        dobot_connect("COM4");
        move(ptpCmd, commandIndex, cmdIdx, 200, 100, 0, 0);
        move(ptpCmd, commandIndex, cmdIdx, 200, 100, -35, 0);
        DobotDll.instance.SetEndEffectorGripper(true, false, true, commandIndex);
        Thread.sleep(500);
        DobotDll.instance.SetEndEffectorGripper(false, false, true, commandIndex);
        move(ptpCmd, commandIndex, cmdIdx, 200, 0, 0, 0);
        DobotDll.instance.DisconnectDobot();


        // DOBOT接続解除
        DobotDll.instance.SetQueuedCmdClear();
        DobotDll.instance.DisconnectDobot();
    }

    private static void Msg(String string) {
        System.out.println(string);
    }
}
0
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
1