Back to page

− Links

 Print 

Dynamixel Library :: Besttechnology

knowledge:Dynamixel Library

Table of contents
    • 概要
    • ライブラリおよびサンプルプログラムのダウンロード
    • 開発環境毎の設定
      • GCC Developer Lite
        • DLLの静的リンク
        • DLLの動的リンク
      • Microsoft Visual C++
      • National Instruments LabVIEW
        • 関数パレットへの登録
      • DELPHI
      • VB
      • Ruby
      • Python
      • Java
      • MathWorks MATLAB
      • Linux
    • API
      • DX_OpenPort
      • DX_ClosePort
      • DX_SetBaudrate
      • DX_Active
      • DX_SetTimeOutOffset
      • DX_Ping
      • DX_Ping2
      • DX_ReadByteData
      • DX_WriteByteData
      • DX_ReadWordData
      • DX_WriteWordData
      • DX_ReadLongData
      • DX_WriteLongData
      • DX_ReadBlockData
      • DX_WriteBlockData
      • DX_ReadSyncData
      • DX_WriteSyncData
      • DX_TxPacket
      • DX_RxPacket
      • DXLIBのオリジナルな定義
    • 追加API

概要 anchor.png[1]

Dynamixel Protocol LibraryはDynamixel Protocol 1の通信プロトコルをサポートした製品をWindows等のOSから操作するためのライブラリ集です。
シリアル通信に関するAPI、タイミングやエラー処理、プロトコルの整合性チェック等を本ライブラリ内で行っているため、シリアル通信である事をほとんど意識すること無くアプリケーションの作りこみに専念することができます。

なお、PCとBTE061D[2]BTE061E[3]BTE068[4]BTE068B[5]BTE082[6]BTE083[7]BTE074[8]BTE079[9]BTE080[10]BTE079B[11]BTE080B[12]BTX229[13]のいずれかがUSBポートに接続され、PCのOSに仮想COMポートが増設された状態で使用するものとします。

Page Top

ライブラリおよびサンプルプログラムのダウンロード anchor.png[14]

以下のリンクよりライブラリ及びサンプルプログラムをアーカイブしたファイルがダウンロードできます。

  • 2019/03/27 Ver.4.2
    内容
    • 追加APIの正式リリース
    • C言語による追加APIのサンプルプログラムを拡充
    • Pythonによるサンプルプログラムを拡充
  • 2016/07/13 Ver.3.3
    内容
    • Linux上でのttyの初期化処理を修正
    • Linux上でのスリープ命令をusleepに置換する様に変更
  • 2016/01/12 Ver.3.2
    内容
    • ヘッダファイルを最近のVCで使用するとエラーになるのを修正
    • DLLMainの不用意な呼び出しで余計な処理をしないよう対応
    • デバイス毎に異なるパケットサイズの制限のうち最小に合わせていたが、ライブラリ内では250バイトとする
    • GCCがやたらエラーを吐くので、ライブラリソースのエンコードをUTF-8に統一
  • 2015/11/18 Ver.3.1
    内容
    • TDxAlarmStatus構造体のアライメントがGCCのバージョンによって1バイト境界にならない事があるのを修正
    • ヘッダファイルの一部の記述がMSVCに対応していなかったのを修正
    • DX_SetBaudrateの処理に待ち時間を挿入
  • 2014/06/11 Ver.3.0
    内容
    • 各APIのアドレス指定が8bit幅だったものを16bitに拡張(後継のDYNAMIXEL2に合わせた)
    • TDxAlarmStatus構造体のアライメントを1バイト境界に変更
    • 不定なDevice IDを指定された際のクラッシュを簡易的にブロック
    • 似非ReadSync APIを追加
    • DX_Reset処理後に1秒の待ち時間を挿入
    • dllのソースをC++に
    • API引数の変更に伴い各サンプルを修正
    • LabVIEWのサンプルを大幅に拡充
    • EXCELのVBAを64bitに対応
    • アーカイブファイル名を変更
  • 2013/10/10 Ver.2.9.1
    内容
    • DX_SetTimeOutOffsetでオフセットタイムを指定していない場合のデフォルト値が0ではタイムアウトが生じやすいため20に変更
    • DX_Ping2の検索最大数を254に修正
    • DXLIB.pasに追加したDX_SetTimeOutOffsetの定義ミスを修正
  • 2013/09/15 Ver.2.9
    内容
    • タイムアウトの値を見定めるのが困難という事から基本的に引数から排除し、内部で理論値を算出する様に変更。
    • タイムアウト以外にタイムラグが必要ということでDX_SetTimeOutOffsetを新設。
    • USB搭載のマイコンのファームにおいてポートオープンの状態検出の仕様を変更したため、DTRの制御をイネーブルに変更。
    • DX_ChangeBaudrateをDX_SetBaudrateに改名。
    • サイズを指定する引数はuint32_tに統一。
    • Linux対応をさらに進め、USB搭載のマイコン等でioctlによるボーレートの変更が使えないケースに一部対応。
    • Win32とLinuxの相違部分を整理して多少見やすくしてみた。
  • 2013/03/28 Ver.2.8.1
    内容
    • DX_Ping2の初期化部において不定なTDevicdIDを想定していなかったのを修正。
  • 2013/03/28 Ver.2.8
    内容
    • 名称をDXLIB2からDXLIBに戻す。
    • ROBO-ONE標準化フォーラムでの総意を元にAPIの定義と型を変更。
      dxlib2.7以前と互換性がなくなるので注意が必要。
      もちろんサンプルも修正。
    • 64ビット環境が増えてきたため、DLL名のサフィックスを統一。
    • Linuxを真面目に対応。
    • 受信スレッドを完全に廃止し処理をブロッキング化し可読性を向上。
    • パケットの整合性チェックを厳密化。
    • デバイスからのアラームステータスがAPIの応答に反映されないケースを修正。
  • 2011/07/20 Ver.2.6
    内容
    • DX_SyncWriteDataをDX_WriteSyncDataへ改名。
    • ByteおよびWordのRead/WriteをBlock Read/Writeで簡素化。
    • 余計な受信データがあった場合でもできるだけ受信処理が進捗する様に改変。
    • 古いバージョンのmatlabがDEVICEIDを誤認するため型を変更。
  • 2011/03/28 Ver.2.5.1
    内容
    • 放置していたLinuxへの対応をマトモに見直し。
  • 2011/03/16 Ver.2.5
    内容
    • 内部のイベント処理をさらに整理しパフォーマンスを改善。
    • イリガルクローズ時のリソースリーク対策。
    • DELPHI向けのサンプル追加。
  • 2011/03/08 Ver.2.4
    内容
    • 内部のイベント処理が一部破綻していたのを修正。
    • シリアルエラー時の停止措置を解除。
    • ゴミデータ受信時の停滞を解消。
  • 2011/03/03 Ver.2.3 内容
    • 内部のイベント処理がことごとく破綻していたのを修正。
    • DX_ChangeBaudrateの返り値が反転していたいのを修正。
    • BROADCASTING IDを使用したPING(DX_Ping2)を追加。
  • 2011/02/02 Ver.2.2 内容
    • ライブラリ構築のオプションを修正し、各言語毎の呼出規約に囚われないファンクション名を吐く様にした。
  • 2011/02/01 Ver.2.1
    内容
    • 呼出規約をstdcallに変更
    • LabVIEWのラッパーviの設定を共通化
    • 新規ターゲット向けにサンプルを追加
  • 2010/12/24 Ver.2.0
    内容
    • 初期リリース

アーカイブファイルには以下のファイルが同梱されます。必要に応じて解凍してください。(※最新版のフォルダ構成を記載しています)

DXLIBdxlib_x32.dllライブラリ本体
dxlib_x64.dll
libdxlib_x32.aGCC用ライブラリ(定義のみ)
libdxlib_x64.a
dxlib_x32.llbMSVC用ライブラリ(定義のみ)
dxlib_x64.lib
dxlib.cppライブラリソース
dxlib.hライブラリヘッダ
dxlib_matlab.hmatlab用ヘッダ
dxlib.pypython用API定義
dxmemmap.hDX/AX/RX/EX/MXシリーズ用コントロールテーブル定義ヘッダ
makelib.batライブラリ再構築用バッチ
83.bat
SampleCodeGCCDeveloperLitesmpl*.cサンプル
dxlib.hDXLIBフォルダに収録されるものと同一
dxmemmap.h
dxlib_x32.dll
dxlib_x64.dll
DELPHIProject1.dprサンプル
Unit1.dfm
Unit1.pas
DXLIB.pas
dxlib_x32.dllDXLIBフォルダに収録されるものと同一
dxlib_x64.dll
LabVIEW2011sample*.viサンプル
DXLIB.llbdllの呼び出しをvi化
DXLIB_DXL.llbdllの呼び出しをvi化
DXLIB_Wrapper.llbアクチュエータに特化したvi
dxlib_x32.dllDXLIBフォルダに収録されるものと同一
EXCELtext.xlsサンプルシート
dxlib_x32.dllDXLIBフォルダに収録されるものと同一
dxlib_x64.dll
Linuxsmpl*.cサンプル
Rubysmpl*.rbサンプル
Pythonsmpl*.pyサンプル
dxlib.pyDXLIBフォルダに収録されるものと同一
dxlib_x32.dll
dxlib_x64.dll
Page Top

開発環境毎の設定 anchor.png[20]

サンプルを元にいくつかのツール・環境で使用する場合の設定方法を説明します。ここで紹介されないツールにおいても、dllを利用できるのであれば必要とされる操作は概ね同様だと思います。、

Page Top

GCC Developer Lite anchor.png[21]

GCC Developer Liteの詳細についてはこちら[22]
'SampleCode\GCCDeveloperLite'フォルダにはAPIの基本的な使い方を紹介したサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。

Page Top
DLLの静的リンク anchor.png[23]

静的にDLLを使用する場合は以下の手順でDLLをリンクする指定を行った上でコンパイルします。なお32bit環境を前提とします。

  1. ファイルの準備

    ダウンロードファイルを解凍後、コンパイルするソースファイルと同一フォルダに以下のファイルをコピー

    ファイルファイル名備考
    ヘッダdxlib.h必要な宣言を集約
    DLLdxlib_x32.dllDLL本体(実行時およびリンク時に必要)
  2. コンパイルオプションの選択

    ツールメニュー→コンパイラオプションをクリックし、表示されるダイアログボックスの設定リストから'x86 (Console)'を選択

    GCC_CompileOption_sel.png
  3. ライブラリの追加

    etc...タブ内の追加ボタンを押し、新規に行を追加

    GCC_CompileOption_AddEtc.png

    新規に追加された空の行をクリックし'dxlib_x32.dll'と入力

    GCC_CompileOption_AddDxlib2.png

OKを押して設定を反映

Page Top
DLLの動的リンク anchor.png[24]

動的にDLLを使用する場合はDLL自体をコンパイラオプションへ追記する必要はありません。その代わりにソース中でdxlib.hをインクルードする前に_DYNAMICLOADマクロを定義しておきます。

#define _DYNAMICLOAD
#include "dxlib.h"

これによりヘッダファイル内の諸定義が切り替わり、DLLのロード及びアンロードを行うLoadDLLとUnloadDLLが利用できるようになります。

  TDeviceID   dev;
  // DLLをロード
  if (LoadDLL ()) {
    if ((dev = DX_OpenPort (COMPORT, BAUDRATE))) {
      ...
      DX_ClosePort (dev);
    }
    // DLLをアンロード
    UnloadDLL ();
  }
Page Top

Microsoft Visual C++ anchor.png[25]

Visual C++ 2010 Express[26]でDLLの静的なリンク方法のみ紹介します。

  1. ファイルの準備

    ダウンロードファイルを解凍後、コンパイルするソースファイルと同一フォルダに以下のファイルをコピー

    ファイルファイル名備考
    ヘッダdxlib.h必要な宣言を集約
    DLLdxlib_x32.dll実行時に必要
    ライブラリdxlib_x32.libリンク時に必要
  2. プロジェクトのプロパティを変更

    構成プロパティ→C/C++→全般→追加のインクルードディレクトリの項目にヘッダファイルの格納フォルダを指定

    vcc_property_includetrq.png

    構成プロパティ→リンカー→全般→追加のライブラリディレクトリの項目にLIBファイルの格納フォルダを指定

    vcc_property_lib_pathtrq.png

    構成プロパティ→リンカー→入力→追加の依存ファイルの項目にLIBファイル名を指定

    vcc_property_lib_filetrq.png

    構成プロパティ→C/C++→プリコンパイル済ヘッダーの項目にプリコンパイル済ヘッダーを使用しないを指定

    vcc_property_precompiletrq.png
Page Top

National Instruments LabVIEW anchor.png[27]

'SampleCode\LabVIEW2011'フォルダにサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
LabVIEW[28]には外部のDLLへアクセスする手段が提供されています。しかしながらDXLIBそのままの定義ではLabVIEWからは使いづらいため、サンプルプログラムではサブviでラッピングしてDXLIB.llbにまとめて提供しています。

vi_frontpanel.png
vi_diagram.png
Page Top
関数パレットへの登録 anchor.png[29]

DXLIB.llbから各viを開き、開いたviの右上にあるアイコンを自分のブロックダイアグラムにドラッグドロップするという操作でviを使用しても良いのですが、頻繁に操作するとなると少々煩雑です。
関数パレットに登録すればスムーズに操作できますので、その手順を紹介します。

  1. LabVIEWのメニューの「ツール」→「上級」→パレットセットを編集」をクリック。
    lvmenu_step1.png
  2. 開いた関数パレットの空いたスペースで右クリックし、ポップアップメニューから「挿入」→「サブパレット」をクリック。
    lvmenu_step2.png
  3. 開いたダイアログボックスから「LLBへリンク(.llb)」を選びOKを押す。
    lvmenu_step3.png
  4. LLBを選択するファイルダイアログボックスから、先に解凍したファイルの中から「DXLIB.llb」を選択しOKを押す。
    lvmenu_step4.png
  5. 次の様なメニューが追加されたらOK。
    lvmenu_step5.png
Page Top

DELPHI anchor.png[30]

'SampleCode\DELPHI'フォルダにサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
DELPHI[31]はPASCAL言語によるPC向け開発ツールであり、外部のDLLへ容易にアクセスする事が出来ます。サンプルに含まれるDXLIB.pas内にdxlib_x32.dllないしdxlib_x64.dllを動的にロードする関数を用意しましたので、ユーザソースのuses節にdxlibを追記すればDynamixel Libraryの各APIへ簡便にアクセスできます。

Page Top

VB anchor.png[32]

'SampleCode\EXCEL'フォルダにサンプルが同梱されます。
ここではVBと似たMicrosoft OfficeのVBAを使用し、マクロの標準モジュールにDXLIB_HEADという名称でDLLに含まれるいくつかのAPIが定義してあります。
Module1にPingTestとMotorTestというマクロが記述されていますので、ワークシートから適宜マクロを呼び出して実行してください。結果がシート上に反映されます。

なお、Officeにも32bit版と64bit版があるため、Declare宣言の方法が異なります。その部分はDXLIB_HEADにて判定していますので、参考にしてください。

Page Top

Ruby anchor.png[33]

'SampleCode\Ruby'フォルダにサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
Ruby[34]はオープンソースの動的なプログラミング言語で、外部のDLLへ簡易にアクセスすることが出来ます。

 require 'dl/import'
 molude dxlib
   extend DL::Importer
   dlload "./dxlib_x32.dll"
   extern "int DX_OpenPort( char *, long )"
 end
 devid = dxlib.DX_OpenPort( "ポート名", ボーレート )
Page Top

Python anchor.png[35]

'SampleCode\Python'フォルダにサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
Python[36]はオープンソースの動的なプログラミング言語で、外部のDLLへ簡易にアクセスすることが出来ます。
各APIのPython用の宣言を定義したdxlib.pyをimportするだけで済みます。

from dxlib import *   # dxlibをインポート
Page Top

Java anchor.png[37]

Page Top

MathWorks MATLAB anchor.png[38]

MATLAB[39]からの使用例を紹介します。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して下さい。また、mex-setupにてCコンパイラを選択[40]しておく必要があります。

  1. 事前準備

    まずはMATLAB起動後、「ファイル(F)」→「パス設定(H)」でdxlib_x32.dll(MATALBが64bitの場合はdxlib_x64.dll)とdxlib_matlab.hの格納されたパスを指定します。dxlib.hはMATLABでは解釈できない記述が多いため使用しないでください。

  2. DLLのロード
    loadlibrary('dxlib_x32.dll','dxlib_matlab.h','alias','dxlib')
  3. ポートのオープン

    ロードされたdxlibのDX_OpenPortを呼出します。関数名の後はポートとボーレートです。

    devid = calllib('dxlib','DX_OpenPort','\\.\COM3',1000000)
  4. TErrorCodeの取得

    TErrorCodeはポインタで引き渡しているため、事前に型宣言をしておきます。値はErr.Valueで取得可能です。必要なければ0を指定しても構いません。

    Err = libpointer('uint16Ptr', 0);
    Ret = calllib('dxlib', 'DX_Ping', devid, 1, Err);
    fprintf('%x', Err.Value);
  5. ポートのクローズ

    DX_OpenPortを行ったので、DX_ClosePortを使用してポートを閉じます。

    calllib('dxlib','DX_ClosePort',devid)
  6. DLLのアンロード
    unloadlibrary('dxlib')
Page Top

Linux anchor.png[41]

Linux上でdxlibをコンパイルする方法を紹介します。

  1. コンパイル準備
    ダウンロードファイルを解凍
  2. ライブラリファイルの生成
    $ gcc -c -D__MAKE_LIB__ dxlib_intuitive.cpp dxlib.cpp
    $ ar -rcsv libdxlib.a dxlib_intuitive.o dxlib.o
  3. 共有ライブラリの作成
    RubyやPython等で使用する場合等
    $ gcc -fPIC -shared -D__MAKE_LIB__ dxlib_intuitive.cpp dxlib.cpp -o dxlib.so.4.2
  4. ユーザプログラムとのリンク
    生成したライブラリファイルと自身のソースをリンクする。
    ポート・ボーレート等は使用する環境に合わせて適宜追加・修正する。
    ライブラリは必要に合わせて追加する。
    $ gcc sample.c -ldxlib -o sample

なお、コンパイルや実行にあたってI/Fやカーネル・ディストリビューションに依存するのがLinuxですので、そのまま使用できない場合は適宜ソースを修正下さい。

Page Top

API anchor.png[42]

本ライブラリはシリアル通信を行うコードを記述しなくて良いのですが、デバイスIDという番号を用いて使用するシリアルI/Fを一元的に管理します。また、制御対象となるDynamixelのIDを指定し、そのコントロールテーブルへの読み書き行うAPIが用意されています。
自身のC言語で記述されたソースプログラムにdxlib.hをインクルードする事で、APIを使用するのに必要なプロトタイプとマクロの定義がなされます。

注意事項
DynamixelのStatus Return Levelを必ず2に設定した上で使用する事。それ以外の値が設定されていた場合はAPIが想定した応答が得られず、タイムアウトするまでAPIから返らない。

Page Top

DX_OpenPort anchor.png[43]

ライブラリの内部情報を初期化すると同時に指定されたCOMポートをオープンし、DX_SetBaudrateを使用して通信速度を設定した後、ユニークなTDeviceIDを返す。以後はこのTDeviceIDを使用して各APIを使用する。
複数のCOMポートを使用する場合は、使用するポート毎にDX_OpenPortを行いTDeviceIDを取得しなくてはならない。
なお、Linuxにおけるボーレートの指定に関しては、DX_SetBaudrateの解説に注意の事。

TDeviceID DX_OpenPort (char *name, uint32_t baud);
  • パラメータ
    • char *name

      インターフェースが提供するCOMポート名。
      記述方法はこちら[44]の情報に従う。

    • long baud

      インターフェースとデバイス間の通信速度[bps]。

  • 戻り値
    • TDeviceID

      オープンに成功した場合は0以外の値、失敗した場合は0を返す。

  • 使用例
    TDeviceID dev;
    // COM10を9600bpsでオープン
    dev = DX_OpenPort ("\\\\.\\COM10", 9600);
Page Top

DX_ClosePort anchor.png[45]

DX_OpenPortで開いたCOMポートを閉じる。
DX_ClosePortが実行された以後は指定されたTDeviceIDでの通信が行えなくなる。

bool DX_ClosePort (TDeviceID dvid);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

  • 戻り値
    • bool

      クローズに成功した場合はtrue、失敗した場合はfalseを返す。

  • 使用例
    TDeviceID dev;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 9600);
    if (dev) {
      ... (中略)
      // クローズ
      DX_ClosePort (dev);
    }
Page Top

DX_SetBaudrate anchor.png[46]

既にオープンされているTDeviceIDの通信速度の変更を行う。
実行すると強制的に受信バッファがクリアされる。
なお、Linux環境におけるボーレートの設定は、POSIX.1でサポートする値(50, 75, 110, 134, 150, 200, 300, 600, 1200, 1800, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1152000, 1500000, 2000000, 2500000, 3000000, 3500000, 4000000)であればtcsetattrを使用して処理するが、これらの値に当てはまらない場合はioctrlを使用する。その際I/Fがこれらのボーレートに対応していなかったり、ioctrlをサポートしない場合、本APIは失敗する。

bool DX_SetBaudrate (TDeviceID dvid, long baud);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • long baud

      新しい通信速度[bps]。

  • 戻り値
    • bool

      通信速度の変更が成功するとtrue、失敗するとfalseを返す。

  • 使用例
    TDeviceID dev;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 9600);
    if (dev) {
      // 通信速度を1M[bps]に変更
      DX_SetBaudrate (dev, 1000000);
      ... (中略)
      // クローズ
      DX_ClosePort (dev);
    }
Page Top

DX_Active anchor.png[47]

指定されたTDeviceIDのポートが開かれており、使用可能であるかを確認する。
USB接続等によりインターフェース自体が取り外し可能な場合に、実際に使用可能であるかを判断するために使用するが、状況によっては正確に判断できない場合もある。

bool DX_Active (TDeviceID dvid);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

  • 戻り値
    • bool

      指定されたdvidが使用可能な場合はtrue、使用不可の場合はfalseを返す。

  • 使用例
    TDeviceID dev;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 9600);
    if (dev) {
      while (DX_Active (dev)) {
        ... (中略)
      }
      // クローズ
      DX_ClosePort (dev);
    }
Page Top

DX_SetTimeOutOffset anchor.png[48]

I/FやOSの都合で生じるであろうタイムラグを予め設定する。
ライブラリ内で想定した受信時間と指定されたタイムアウトオフセット時間を加算した時間を超えた場合に、タイムアウトエラーとして処理される。
デフォルトは20。

void DX_SetTimeOutOffset (TDeviceID dvid, uint32_t offsettime);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint32_t offsettime

      タイムアウトオフセット時間[ms]

  • 戻り値
    • bool

      指定されたdvidが使用可能な場合はtrue、使用不可の場合はfalseを返す。

Page Top

DX_Ping anchor.png[49]

PINGインストラクションを使用して対象IDからの応答を確認する。

bool DX_Ping (TDeviceID dvid, uint8_t id, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~254)。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 57143);
    if (dev) {
      // ID=1にPINGを発行
      if (DX_Ping (dev, 1, &err))
        printf ("Found [%08X]\n", err);
      else
        printf ("Not found [%08X]\n", err);
      // クローズ
      DX_ClosePort (dev);
    }
Page Top

DX_Ping2 anchor.png[50]

PINGインストラクションでBROADCASTING IDを指定して不特定の対象の応答を確認する。
応答時間がDynamixelの種類によって差があるため、それらが混在している環境では正確な情報を取得できない場合がある。

bool DX_Ping2 (TDeviceID dvid, uint32_t *num, TDxAlarmStatus *AlarmStatus, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint32_t *num

      検索する最大数及び検索で見つかったデバイス数の保存先。

    • TDxAlarmStatus *AlarmStatus

      検索で見つかったデバイス情報の保存先。
      少なくともnumで指定したサイズを確保しておく必要がある。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      1台以上のデバイスからの応答が得られた場合はtrue、それ以外はfalseを返す。

  • 使用例
    TDeviceID   dev;
    TErrorCode  err;
    uint8_t     id;
    TDxAlarmStatus stat[254];
    int         i;
    uint32_t    num = 254;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 57143);
    if (dev) {
      // 不明な対象に対してPINGを発行
      // numには予め254の値を指定したので、最大254台まで見つけようとする
      if (DX_Ping2 (dev, &num, stat, &err)) {
        // 1台以上見つかるとnumには見つかった数が返ってくる
        for (i = 0; i < num; i++)
          printf ("Found ID=%d %02X\n", stat[i].id, stat[i].Status);
      }
      // クローズ
      DX_ClosePort (dev);
    }
Page Top

DX_ReadByteData anchor.png[51]

対象IDのコントロールテーブルから1バイトのデータを読み出す。

bool DX_ReadByteData(TDeviceID dvid, uint8_t id, uint16_t adr, uint8_t *rdata, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~253)。

    • uint16_t adr

      コントロールテーブルのアドレス。

    • uint8_t *rdata

      読み出した値の保存先。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    uint8_t    dat;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のAX-12からLEDの状態を取得
      if (DX_ReadByteData (dev, 1, 25, &dat, &err)) {
        printf ("LED STAT=%d\n", dat);
      }
      DX_ClosePort (dev);
    }
Page Top

DX_WriteByteData anchor.png[52]

対象IDのコントロールテーブルへ1バイトのデータを書き込む。

bool DX_WriteByteData(TDeviceID dvid, uint8_t id, uint16_t adr, uint8_t dat, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~253, 254)。

    • uint16_t adr

      コントロールテーブルのアドレス。

    • uint8_t dat

      書き込む値。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。
      BROADCASTING IDを指定した場合は応答待ちを行わない。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    uint8_t    dat;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のAX-12からLEDの状態を取得
      if (DX_ReadByteData (dev, 1, 25, &dat, &err)) {
        dat ^= 1; // ビット反転
        // ID=1のAX-12へLEDの状態を設定
        DX_WriteByteData (dev, 1, 25, dat, &err);
      }
      DX_ClosePort (dev);
    }
Page Top

DX_ReadWordData anchor.png[53]

対象IDのコントロールテーブルから1ワード(2バイト)のデータを読み出す。

bool DX_ReadWordData(TDeviceID dvid, uint8_t id, uint16_t adr, uint16_t *rdata, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~253)。

    • uint16_t adr

      コントロールテーブルのアドレス。

    • uint16_t *rdata

      読み出した値の保存先。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    uint16_t   dat;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のAX-12から現在位置を取得
      if (DX_ReadWordData (dev, 1, 36, &dat, &err)) {
        printf ("PRESENT POS=%d\n", dat);
      }
      DX_ClosePort (dev);
    }
Page Top

DX_WriteWordData anchor.png[54]

対象IDのコントロールテーブルへ1ワード(2バイト)のデータを書き込む。

bool DX_WriteWordData(TDeviceID dvid, uint8_t id, uint16_t adr, uint16_t dat, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~253, 254)。

    • uint16_t adr

      コントロールテーブルのアドレス。

    • uint16_t dat

      書き込む値。

    • TErrorCode *errcode

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。
      BROADCASTING IDを指定した場合は応答待ちを行わない。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のAX-12へ位置(511)を指令
      DX_WriteWordData (dev, 1, 30, 511, &err);
      DX_ClosePort (dev);
    }
Page Top

DX_ReadLongData anchor.png[55]

対象IDのコントロールテーブルから2ワード(4バイト)のデータを読み出す。

bool DX_ReadLongData(TDeviceID dvid, uint8_t id, uint16_t adr, uint32_t *rdata, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~253)。

    • uint16_t adr

      コントロールテーブルのアドレス。

    • uint32_t *rdata

      読み出した値の保存先。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    uint32_t   dat;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のXM430から現在位置を取得
      if (DX_ReadLongData (dev, 1, 132, &dat, &err)) {
        printf ("PRESENT POS=%d\n", dat);
      }
      DX_ClosePort (dev);
    }
Page Top

DX_WriteLongData anchor.png[56]

対象IDのコントロールテーブルへ2ワード(4バイト)のデータを書き込む。

bool DX_WriteLongData(TDeviceID dvid, uint8_t id, uint16_t adr, uint32_t dat, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~253, 254)。

    • uint16_t adr

      コントロールテーブルのアドレス。

    • uint32_t dat

      書き込む値。

    • TErrorCode *errcode

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。
      BROADCASTING IDを指定した場合は応答待ちを行わない。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    // オープン
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のXM430へ位置(1024)を指令
      DX_WriteLongData (dev, 1, 30, 1024, &err);
      DX_ClosePort (dev);
    }
Page Top

DX_ReadBlockData anchor.png[57]

対象IDのコントロールテーブルから指定サイズのデータを読み出す。

bool DX_ReadBlockData (TDeviceID dvid, uint8_t id, uint16_t adr, uint8_t *rdata, uint32_t len, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~253)。

    • uint16_t adr

      コントロールテーブルのアドレス。

    • uint8_t *rdata

      読み出したデータの保存先。

    • uint32_t len

      読み出すデータのサイズ。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    uint8_t    comp[4];
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のAX-12+からコンプライアンスのデータを取得
      if (DX_ReadBlockData (dev, 1, 26, comp, 4, &err) {
        printf (
         "CWM=%d CCWM=%d CWS=%d CCWS=%d\n",
         comp[0], comp[1], comp[2], comp[3]
        );
      }
      DX_ClosePort (dev);
    }
Page Top

DX_WriteBlockData anchor.png[58]

対象IDのコントロールテーブルへ指定サイズのデータを書き込む。

bool DX_WriteBlockData(TDeviceID dvid, uint8_t id, uint16_t adr, uint8_t *dat, uint32_t len, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~253, 254)。

    • uint16_t adr

      コントロールテーブルのアドレス。

    • uint8_t *dat

      書き込むデータの保存先。

    • uint32_t len

      書き込むデータのサイズ。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      正常な応答が得られた場合はtrue、それ以外はfalseを返す。
      BROADCASTING IDを指定した場合は応答待ちを行わない。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    uint8_t    comp[4] = { 0, 1, 200, 200 };
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のAX-12+のコンプライアンスを変更
      DX_WriteBlockData (dev, 1, 26, comp, 4, &err);
      DX_ClosePort (dev);
    }
Page Top

DX_ReadSyncData anchor.png[59]

SYNCインストラクションには読み出し機能が無いが、DX_ReadBlockDataを使用して指定された複数IDからブロック読み出しを行う。正確には「同期」していない。

bool DX_ReadSyncData (TDeviceID dvid, uint8_t *dat, uint32_t size, uint8_t *retdat, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t *dat

      読み出す情報の保存先
      [addr][length][ID1][ID2]...[IDn]
      addr:先頭アドレス (1バイト)
      length:先頭アドレスからのバイト数 (1バイト)
      IDn:読み出す対象IDを列挙 (nバイト)。

    • uint32_t size

      datのサイズ。

    • uint8_t result
      読み出したデータの保存先
      [ID1][ALARM1][DATA1_0]....[DATA1_m]
      [ID2][ALARM2][DATA2_0]....[DATA2_m]
      ... [IDn][ALARMn][DATAn_0]....[DATAn_m]
      IDn:対象ID (1バイト)
      ALARMn:TErrorCode (2バイト)
      DATAn_m:読み出されたデータ
    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      指定された全てのIDに対する読み出しが正常に完了したらtrue、それ以外はfalseを返す。

Page Top

DX_WriteSyncData anchor.png[60]

SYNCインストラクションを使用して複数IDへブロック書き込みを行う。
書き込まれるデータの構成はユーザに委ねられる。

bool DX_WriteSyncData (TDeviceID dvid, uint8_t *dat, uint32_t size, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t *dat

      書き込むパラメータの保存先。

    • uint32_t size

      パラメータのサイズ。

    • TErrorCode *err

      エラーコード。

  • 戻り値
    • bool

      インターフェースより送信が行われた場合はtrue、それ以外はfalseを返す。

  • 使用例
    #define _POS1 (400)
    #define _POS2 (511)
    
    TDeviceID  dev;
    TErrorCode err;
    uint8_t    param[8] = {
      30,    // アドレス (Goal Position)
      2,     // データ長 (2 byte)
      // ID=1用データ
      1,
      _POS1 & 0xff,
      _POS1 >> 8,
      // ID=2用データ
      2,
      _POS2 & 0xff,
      _POS2 >> 8
    };
    
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // 2つのAX-12へ同時に位置を指令
      DX_WriteSyncData (dev, param, 8, &err);
      DX_ClosePort (dev);
    };
Page Top

DX_TxPacket anchor.png[61]

任意のインストラクションパケットを送信する。

bool DX_TxPacket (TDeviceID dvid, uint8_t id, TInstruction inst, uint8_t *param, uint32_t len, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~254)。

    • TInstruction inst

      使用するインストラクション。

    • uint8_t *param

      送信するパラメータの保存先。

    • uint32_t len

      送信するパラメータのサイズ。

    • TErrorCode *err

      エラーコード。

  • 戻り値

    インターフェースより送信が行われた場合はtrue、それ以外はfalseを返す。

  • 使用例
    TDeviceID  dev;
    TErrorCode err;
    uint8_t    param[2] = {
      25,    // アドレス (LED)
      0,     // データ
    };
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のAX-12+のLEDを消灯
      DX_TxPacket (dev, 1, INST_WRITE, param, 2, &err);
      DX_ClosePort (dev);
    };
Page Top

DX_RxPacket anchor.png[62]

ステータスパケットを受信する。
基本的にDX_TxPacketとペアで使用する。ステータスパケットが得られない状況で使用するとタイムアウトするまで返らない。
なお、本APIはDX_SetTimeOutOffsetで設定されたオフセット値は使用せず、引数で指定された受信タイムアウトのみが適用される。

bool DX_RxPacket (TDeviceID dvid, uint8_t *rdata, uint32_t rdatasize, uint32_t *rlen, uint32_t timeout, TErrorCode *err);
  • パラメータ
    • TDeviceID dvid

      DX_OpenPortで開いた際のTDeviceID。

    • uint8_t *rdata

      受信バッファ。
      ステータスパケットを受信するのに十分なサイズを確保しておく必要がある。

    • uint32_t readsize

      rdataのサイズ。

    • uint32_t *rlen

      実際に受信されたステータスパケットのサイズ。

    • int timeout

      受信タイムアウト[ms]。

    • TErrorCode *err

      エラーコード。

  • 戻り値

    受信成功時はtrue、それ以外はfalseを返す。

  • 使用例
    int        i;
    uint32_t   len;
    TDeviceID  dev;
    TErrorCode err;
    uint8_t    param[2] = {
      25,    // アドレス (LED)
      1,     // サイズ
    };
    
    dev = DX_OpenPort ("\\\\.\\COM10", 1000000);
    if (dev) {
      // ID=1のAX-12+からLEDの状態を読み出す要求
      if (DX_TxPacket (dev, 1, INST_READ, param, 2, &err)) {
        // ステータスパケットを受信
        DX_RxPacket (dev, dat, sizeof (dat), &len, 100, &err);
        for (i = 0; i < len; i++) {
          printf ("[%02X]", dat[i]);
        }
      }
      DX_ClosePort (dev);
    };
Page Top

DXLIBのオリジナルな定義 anchor.png[63]

TDeviceID
(uint32_t|uint64_t)
インターフェース毎に割り当てられるユニークな値。DX_OpenPortにて自動的に生成される。
TInstruction
(uint8_t)
DX_TxPacketにてインストラクションパケットを送信する場合に使用される。
使用可能なマクロは以下の通り。

INST_PING
INST_READ
INST_WRITE
INST_REG_WRITE
INST_ACTION
INST_RESET
INST_SYNC_WRITE
INST_SYNG_REG_WRITE

TDxAlarmStatus
struct {
uint8_t id;
TErrorCode Status;
}
idとTErrorCodeを対にした構造体。
TErrorCode
(uint16_t)
API内で検出される16ビットのエラーコード。上位8ビットはAPI内部で検出したエラー、下位8ビットはステータスパケットに含まれるエラーフラグが割り当てられている。
bitmacro name
15ERR_INVALID_DEVID使用できないTDeviceID
14ERR_INVALID_ID指定できないID
13ERR_DIFF_ID異なるIDからの応答
12ERR_ILLEGAL_SIZE異常なデータサイズ
11ERR_INVALID_PARAM異常なパラメータ
10ERR_COMMシリアルポートエラー
9ERR_CHECKSUM異常なチェックサム
8ERR_TIMEOUT受信タイムアウト
7
6ERR_DX_INST未定義のインストラクションが指定された、もしくはreg_writeなしでactionが指定された
5ERR_DX_OVERLOAD指定された最大トルクで現在の負荷を制御できない
4ERR_DX_CHECKSUMインストラクションパケットのチェックサムが正しく無い
3ERR_DX_RANGEパラメータの設定範囲を超えた
2ERR_DX_OVERHEAT内部温度が設定温度を超えた
1ERR_DX_ANGLEAngle Limitの範囲外にGoal Positionが指定された
0ERR_DX_OVERVOLTAGE電源電圧が指定動作電圧の範囲を超えた

Page Top

追加API anchor.png[64]

使用するDynamixelの情報を予めライブラリ内に保持させておき、位置や角速度等の指令およびフィードバック値は物理値を扱い、各々の区別はIDのみで行うAPIです。
これAPIによりDynamixelのモデルごとに異なるコントロールテーブルや煩雑な運転方法の違いを意識する事なく、少ないコードで目的の挙動を実現できます。
なお、従来通りコントロールテーブルへの直接アクセスを制限するものではありませんが、その場合は本機能が想定する状況と一致しなくなる事があります。
※以下の使用例はDX2LIB[65]を前提としています。

Table of contents
      • 初期設定関連
        • DXL_ScanDevices
        • DXL_GetModelInfo
        • DXL_PrintDevicesList
        • DXL_InitDevicesList
      • エラー
        • DXL_GetErrorCode
        • DXL_GetHWErrorCode
      • 動作モード
        • DXL_SetOperatingMode
        • DXL_SetOperatingModesEquival
        • DXL_GetOperatingMode
      • LED
        • DXL_SetLED
      • 制御ON/OFF
        • DXL_SetTorqueEnable
        • DXL_SetTorqueEnables
        • DXL_SetTorqueEnablesEquival
        • DXL_GetTorqueEnable
        • DXL_GetTorqueEnables
      • 角度
        • DXL_SetGoalAngle
        • DXL_SetGoalAngles
        • DXL_GetPresentAngle
        • DXL_GetPresentAngles
      • 停止
        • DXL_StandStillAngle
        • DXL_StandStillAngles
      • 角速度
        • DXL_SetGoalVelocity
        • DXL_SetGoalVelocities
        • DXL_GetPresentVelocity
        • DXL_GetPresentVelocities
      • 角度と角速度
        • DXL_SetGoalAngleAndVelocity
        • DXL_SetGoalAnglesAndVelocities
      • 角度と時間
        • DXL_SetGoalAngleAndTime
        • DXL_SetGoalAnglesAndTime
      • 電流
        • DXL_SetGoalCurrent
        • DXL_SetGoalCurrents
        • DXL_GetPresentCurrent
        • DXL_GetPresentCurrents
      • PWM
        • DXL_SetGoalPWM
        • DXL_SetGoalPWMs
        • DXL_GetPresentPWM
        • DXL_GetPresentPWMs
      • オリジナルな定義

初期設定関連 anchor.png[66]

Dynamixelが持つ固有の情報を予め記憶させておくことで、それ以後の操作はIDのみを指定し、どのモデルであっても使用頻度が高いアイテムはコントロールテーブルを意識する事無く同じAPIで同様の動作ができます。

Page Top
DXL_ScanDevices anchor.png[67]

ID0~252のデバイスを順次検索し、サポートするDynamixelであればライブラリ内のリストに登録する。
リストに登録されていないデバイスのIDに対しては存在しないものとして通信を行わないため、DXL_ScanDevicesかDXL_GetModelInfoを用いて予めリストに登録しておく必要がある。

int DXL_ScanDevices (TDeviceID dvid, uint8_t *ids);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      見つかったデバイスのID一覧を必要に応じて取得する配列の保存先。最小でも253バイトを確保すること。

  • 戻り値
    • int

      見つかったデバイスの数。

  • 使用例
    #include <stdio.h>
    #include <dx2lib.h>
    
    void main (void) {
     // COM10を57600bpsでオープン
     TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
     if (dev != 0) {
       // ID=0~252の情報を取得し成功したもののみライブラリ内のリストに追加
       printf("detect num=%d\n", DXL_ScanDevices (dev, NULL));
       DX2_Close (dev);
     }
    }
    
Page Top
DXL_GetModelInfo anchor.png[68]

指定IDのモデル情報の取得し、サポートするDynamixelであればライブラリ内のリストに登録する。
リストに登録されていないデバイスのIDに対しては存在しないものとして通信を行わないため、DXL_ScanDevicesかDXL_GetModelInfoを用いて予めリストに登録しておく必要がある。

PDXL_ModelInfo DXL_GetModelInfo (TDeviceID dvid, uint8_t id);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

  • 戻り値
    • PDXL_ModelInfo

      登録されたIDと一致するTDXL_ModelInfoのアドレス。

  • 使用例
    #include <stdio.h>
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("/dev/ttyUSB0", 57600);
      for (uint8_t id = 0; id <= 252; id++) {
        // idの情報を取得し成功したらライブラリ内のリストに追加
        PDXL_ModelInfo p = DXL_GetModelInfo (dev, id);
        if (p->modelno != 0)
          printf ("[%3d] %s ($%04X) %d\n", id, p->name, p->modelno, p->devtype);
        }
      }
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_PrintDevicesList anchor.png[69]

ライブラリ内のリストに登録された全てのデバイスの情報をコンソールに出力する。
ID・モデル名・モデル番号の順に出力される。

bool DXL_PrintDevicesList (int (*pf) (const char *, ...));
  • パラメータ
    • int (*pf)(const char *, ...)

      printf等の書式化文字列出力ルーチンのポインタ。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <stdio.h>
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("/dev/ttyUSB0", 57600);
      DXL_ScanDevices (dev, NULL);
      DXL_PrintDevicesList ((void *)&printf);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_InitDevicesList anchor.png[70]

ライブラリ内に保持されたデバイスのリストをクリアする。

void DXL_InitDevicesList (void);
Page Top

エラー anchor.png[71]

Page Top
DXL_GetErrorCode anchor.png[72]

追加APIの処理の中で最後に取得したエラーを返す。本API自体は通信を行わない。

TErrorCode DXL_GetErrorCode (TDeviceID dvid, uint8_t id);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

  • 戻り値
    • TErrorCode

      エラーコード。

Page Top
DXL_GetHWErrorCode anchor.png[73]

対象IDのコントロールテーブルにハードウェアエラーが備わっている場合にのみ取得する。

bool DXL_GetHWErrorCode (TDeviceID dvid, uint8_t id, uint8_t *hwerr);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • uint8_t *hwerr

      取得されたハードウェアエラーの保存先。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

動作モード anchor.png[74]

現在のところDynamixel X・PRO+シリーズが最も沢山の動作モードをサポートしています。概ねこの動作モードによって動作の大別が行われるため、他のモデルにおいてもこの動作モードの番号を踏襲することとします。

Page Top
DXL_SetOperatingMode anchor.png[75]

対象IDの動作モードを変更する。
指定されたモードと現在のモードが異なる場合にのみトルクをOFFにし動作モードを更新する。
対称IDがライブラリ内のリストに登録されている必要がある。

bool DXL_SetOperatingMode (TDeviceID dvid, uint8_t id, uint8_t mode);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • uint8_t mode

      Dynamixel X/PRO/PRO plusシリーズではmodeの値をそのまま適用、それ以外では近似した動作モードを適用。
      0: Current Control (電流制御)
      1: Velocity Control (速度制御)
      3: Position Control (位置制御)
      4: Expand Position Control (多回転位置制御)
      5: Current-Base Position Control
      16: PWM Control

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      // ID:1を検索し見つかれば登録
      DXL_GetModelInfo (dev, 1);
      // ID:1をCurrent-Base Position Controlに設定
      DXL_SetOperatingMode (dev, 1, 5);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_SetOperatingModesEquival anchor.png[76]

複数IDの動作モードを一括変更する。
指定されたモードと現在のモードが異なる場合にのみトルクをOFFにし動作モードを更新する。
ライブラリ内のリストに登録されていないIDは無視される。

bool DXL_SetOperatingModesEquival (TDeviceID dvid, const uint8_t *ids, int num, uint8_t mode);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • int num

      対象とするID数

    • uint8_t mode

      Dynamixel Xシリーズの動作モード[77]を想定し、それに完全対応しないデバイスに対しては近似値もしくは失敗を返す。
      0: Current Control
      1: Velocity Control
      3: Position Control
      4: Expand Position Control
      5: Current-Base Position Control
      16: PWM Control

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      uint8_t ids[3] = {1, 2, 3};
      // ID:1~3を検索し見つかれば登録
      for (int i = 0; i < sizeof (ids); i++) DXL_GetModelInfo (dev, ids[i]);
      // ID:1~3をVelocity Controlに設定
      DXL_SetOperatingModesEquival (dev, ids, sizeof (ids), 1);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_GetOperatingMode anchor.png[78]

対象IDの動作モードを取得する。

bool DXL_GetOperatingMode (TDeviceID dvid, uint8_t id, uint8_t *mode);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • uint8_t *mode

      取得した動作モードの保存先。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

LED anchor.png[79]

Dynamixelに備わっているLEDは全てのモデルにおいてコントロールテーブル上の同じアドレスに割り当てがなされている訳では無いため、デバッグに有用なLEDを一つのAPIで使用できます。

Page Top
DXL_SetLED anchor.png[80]

対象IDのLEDを明滅させる。

bool DXL_SetLED (TDeviceID dvid, uint8_t id, bool en);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • bool en

      trueで点灯、falseで消灯。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

制御ON/OFF anchor.png[81]

コントロールテーブル上でトルクイネーブルと称しているアイテムは、LED同様にモデルによってアドレスが異なります。制御の開始と停止を司るため、こちらも同じAPIで運用できます。

Page Top
DXL_SetTorqueEnable anchor.png[82]

対象IDの制御を開始ないし停止させる。
なお、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetTorqueEnable (TDeviceID dvid, uint8_t id, bool en);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • bool en

      trueで開始、falseで停止。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      DXL_GetModelInfo (dev, 1);
      DXL_SetOperatingMode (dev, 1, 3);
      // ID:1の制御をON
      DXL_SetTorqueEnable (dev, 1, true);
      // ID:1へ角度指令 180deg
      DXL_SetGoalAngle (dev, 1, 180.0);
      // 1000ms待機
      Sleep (1000);
      // ID:1の制御をOFF
      DXL_SetTorqueEnable (dev, 1, false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_SetTorqueEnables anchor.png[83]

複数IDの制御を個別に開始ないし停止させる。
ライブラリ内のリストに登録されていないIDは無視される。
なお、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetTorqueEnables (TDeviceID dvid, const uint8_t *ids, const bool *ens, int num);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • const bool *ens

      idsで指定されたID順にtrueで開始、falseで停止を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_SetTorqueEnablesEquival anchor.png[84]

複数IDの制御を一括で開始ないし停止させる。
ライブラリ内のリストに登録されていないIDは無視される。
なお、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetTorqueEnablesEquival (TDeviceID dvid, const uint8_t *ids, int num, bool en);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • int num

      対象とするID数

    • bool en

      trueで開始、falseで停止。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      DXL_GetModelInfo (dev, 1);
      DXL_GetModelInfo (dev, 2);
      DXL_GetModelInfo (dev, 3);
      uint8_t ids[3] = {1, 2, 3};
      // ID:1~3の制御をON
      DXL_SetTorqueEnablesEquival (dev, ids, sizeof (ids), true);
    
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_GetTorqueEnable anchor.png[85]

対象IDのトルクイネーブル状態を取得する。

bool DXL_GetTorqueEnable (TDeviceID dvid, uint8_t id, bool *en);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • bool *en

      取得したトルクイネーブルの保存先。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_GetTorqueEnables anchor.png[86]

複数IDのトルクイネーブル状態を取得する。
ライブラリ内のリストに登録されていないIDは無視される。

bool DXL_GetTorqueEnables (TDeviceID dvid, const uint8_t *ids, bool *en, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • bool *en

      idsで指定されたID順に取得したトルクイネーブルを保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

角度 anchor.png[87]

DynamixelのコントロールテーブルではPositionと称しているユニークな単位系を持つアイテムは、モデルによって動作角度や分解能が大きく異なります。異なるモデルを混在して運用する場合は非常に不便なため、物理値の角度[deg]を用いてアクセスします。

Page Top
DXL_SetGoalAngle anchor.png[88]

指定IDへ目標角度を指令する。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalAngle (TDeviceID dvid, uint8_t id, double angle);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double angle

      目標角度[deg]。中立位置を0[deg]とする。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      DXL_GetModelInfo (dev, 1);
      DXL_SetOperatingMode (dev, 1, 3);
      DXL_SetTorqueEnable (dev, 1, true);
      // ID:1へ角度指令 180deg
      DXL_SetGoalAngle (dev, 1, 180.0);
      Sleep (1000);
      // ID:1へ角度指令 -180deg
      DXL_SetGoalAngle (dev, 1, -180.0);
      Sleep (1000);
      // ID:1へ角度指令 0deg
      DXL_SetGoalAngle (dev, 1, 0.0);
      Sleep (1000);
      DXL_SetTorqueEnable (dev, 1, false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_SetGoalAngles anchor.png[89]

複数IDへ個別の目標角度を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalAngles (TDeviceID dvid, const uint8_t *ids, const double *angles, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • const double *angles

      idsで指定されたID順に目標角度[deg]を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      DXL_GetModelInfo (dev, 1);
      DXL_GetModelInfo (dev, 2);
      DXL_GetModelInfo (dev, 3);
      uint8_t ids[3] = {1, 2, 3};
      DXL_SetOperatingModesEquival (dev, ids, sizeof (ids), 3);
      DXL_SetTorqueEnablesEquival (dev, ids, sizeof (ids), true);
      // 3軸分の角度を保存したの配列
      double angles[3] = { 0.0, 150.0, 300.0 };
      // 3軸分の角度指令
      DXL_SetGoalAngles (dev, ids, angles, sizeof (ids));
      Sleep (2000);
      DXL_SetTorqueEnablesEquival (dev, ids, sizeof (ids), false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_GetPresentAngle anchor.png[90]

対象IDの現在角度を取得する。

bool DXL_GetPresentAngle (TDeviceID dvid, uint8_t id, double *angle);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double *angle

      取得した現在角度[deg]の保存先。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <stdio.h>
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      uint8_t id = 1;
      DXL_GetModelInfo (dev, id);
      DXL_SetOperatingMode (dev, id, 3);
      DXL_SetTorqueEnable (dev, id, true);
    
      DXL_SetGoalAngle (dev, id, -180.0);
      Sleep (1000);
    
      DXL_SetGoalAngle (dev, id, 180.0);
      for (int i = 0; i < 1000; i++) {
        double pang;
        // 現在角度取得
        if (DXL_GetPresentAngle (dev, id, &pang))
          printf ("%5.1\r", pang);
        Sleep (10);
      }
    
      DXL_SetTorqueEnable (dev, id, false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_GetPresentAngles anchor.png[91]

複数IDの現在角度を取得する。
ライブラリ内のリストに登録されていないIDは無視される。

bool DXL_GetPresentAngles (TDeviceID dvid, const uint8_t *ids, double *angles, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • bool *angles

      idsで指定されたID順に取得した現在角度[deg]を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

停止 anchor.png[92]

制御をOFFにする事で制御が停止するのと合わせて脱力状態になります。それでは現在の角度を保持しつつ止めることができないため、指令されたタイミングの角度を取得してその値をそのまま角度指令し直す機能を設けています。

Page Top
DXL_StandStillAngle anchor.png[93]

指定IDを現在角度で停止させる。
動作モードがPosition Controlである事。

bool DXL_StandStillAngle (TDeviceID dvid, uint8_t id);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_StandStillAngles anchor.png[94]

複数IDを現在角度で停止させる。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事。

bool DXL_StandStillAngles (TDeviceID dvid, const uint8_t *ids, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

角速度 anchor.png[95]

DynamixelのコントロールテーブルではVelocityと称しているユニークな単位系を持つアイテムは、モデルによってレンジや分解能が異なります。異なるモデルを混在して運用する場合は非常に不便なため、物理値の角速度[deg/sec]を用いてアクセスします。

Page Top
DXL_SetGoalVelocity anchor.png[96]

指定IDへ目標角速度を指令する。
動作モードがVelocity Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalVelocity (TDeviceID dvid, uint8_t id, double velocity);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double velocity

      目標角速度[deg/s]。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      DXL_GetModelInfo (dev, 1);
      DXL_SetOperatingMode (dev, 1, 1);
      DXL_SetTorqueEnable (dev, 1, true);
    
      // 角速度指令 30deg/s
      DXL_SetGoalVelocity (dev, 1, 30.0);
      Sleep (1000);
    
      // 角速度指令 40deg/s
      DXL_SetGoalVelocity (dev, 1, 40.0);
      Sleep (1000);
    
      DXL_SetTorqueEnable (dev, 1, false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_SetGoalVelocities anchor.png[97]

複数IDへ個別の目標角速度を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがVelocity Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalVelocities (TDeviceID dvid, const uint8_t *ids, const double *velocities, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • const double *velocities

      idsで指定されたID順に目標角速度[deg/s]を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_GetPresentVelocity anchor.png[98]

対象IDの現在角速度を取得する。

bool DXL_GetPresentVelocity (TDeviceID dvid, uint8_t id, double *velocity);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double *velocity

      取得した現在角速度[deg/s]の保存先。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_GetPresentVelocities anchor.png[99]

複数IDの現在角速度を取得する。
ライブラリ内のリストに登録されていないIDは無視される。

bool DXL_GetPresentVelocities (TDeviceID dvid, const uint8_t *ids, double *velocities, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • bool *velocities

      idsで指定されたID順に取得した現在角速度[deg/s]を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

角度と角速度 anchor.png[100]

角度と合わせて角速度を同時に指令することで、指定角度へゆっくり動かすと行った動作を行わせる事ができます。
なお、Dynamixelの角速度制御の分解能はさほど大きくないため、角速度が低い場合は誤差が大きくなります。

Page Top
DXL_SetGoalAngleAndVelocity anchor.png[101]

指定IDへ目標角度と目標角速度を指令する。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalAngleAndVelocity (TDeviceID dvid, uint8_t id, double angle, double velocity);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double angle

      目標角度[deg]。

    • double velocity

      目標角速度[deg/s]。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      uint8_t id = 3;
      DXL_GetModelInfo (dev, id);
      DXL_SetOperatingMode (dev, id, 3);
      DXL_SetTorqueEnable (dev, id, true);
      // 現在角度から-170degの角度へ30deg/sの角速度で
      DXL_SetGoalAngleAndVelocity (dev, id, -170.0, 30.0);
      Sleep (10000);
      // 現在角度から160degの角度へ40deg/sの角速度で
      DXL_SetGoalAngleAndVelocity (dev, id, 160.0, 40.0);
      Sleep (10000);
      DXL_SetTorqueEnable (dev, id, false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_SetGoalAnglesAndVelocities anchor.png[102]

複数IDへ個別の目標角度と目標角速度を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalAnglesAndVelocities (TDeviceID dvid, const uint8_t *ids, const PAngleVelocity anglevelocity, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • const PAngleVelocity anglevelocity

      idsで指定されたID順に目標角度[deg]と目標角速度[deg/s]を保持する構造体の配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

角度と時間 anchor.png[103]

角度と合わせて時間を同時に指令します。ライブラリ内では時間を角速度(=(目標角度-現在角度)/移動時間)に変換して指令します。
なお、Dynamixelの角速度制御の分解能はさほど大きくないため、移動速度が長い場合は誤差が大きくなります。

Page Top
DXL_SetGoalAngleAndTime anchor.png[104]

指定IDへ目標角度と現在角度から目標角度までの移動時間を指令する。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalAngleAndTime (TDeviceID dvid, uint8_t id, double angle, double sec);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double angle

      目標角度[deg]。

    • double sec

      移動時間[sec]。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      uint8_t id = 3;
      DXL_GetModelInfo (dev, id);
      DXL_SetOperatingMode (dev, id, 3);
      DXL_SetTorqueEnable (dev, id, true);
      // 現在角度から-150degの角度へ3秒で移動
      DXL_SetGoalAngleAndTime (dev, id, -150.0, 3.0);
      Sleep (4000);
      // 現在角度から180degの角度へ5秒で移動
      DXL_SetGoalAngleAndTime (dev, id, 180.0, 5.0);
      Sleep (6000);
      DXL_SetTorqueEnable (dev, id, false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_SetGoalAnglesAndTime anchor.png[105]

複数IDへ個別の目標角度と現在角度から目標角度までの移動時間を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalAnglesAndTime (TDeviceID dvid, const uint8_t *ids, const double *angles, int num, double sec);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • const double *angles

      idsで指定されたID順に目標角度[deg]を保持する配列の保存先。

    • int num

      対象とするID数

    • double sec

      目標移動時間[sec]。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

電流 anchor.png[106]

XシリーズないしPROシリーズのみで使用でき、モデルが異なっていてもできる限り同じ運用ができるよう物理値の電流[mA]を用いてアクセスします。

Page Top
DXL_SetGoalCurrent anchor.png[107]

指定IDへ目標電流を指令する。
プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalCurrent (TDeviceID dvid, uint8_t id, double current);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double current

      目標電流[mA]。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      DXL_GetModelInfo (dev, 1);
      DXL_SetOperatingMode (dev, 1, 0);
      DXL_SetTorqueEnable (dev, 1, true);
      // 電流指令 200mA
      DXL_SetGoalCurrent (dev, 1, 200.0);
      Sleep (1000);
      // 電流指令 -200mA
      DXL_SetGoalAngle (dev, 1, -200.0);
      Sleep (1000);
      DXL_SetTorqueEnable (dev, 1, false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_SetGoalCurrents anchor.png[108]

複数IDへ個別の目標電流を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalCurrents (TDeviceID dvid, const uint8_t *ids, const double *currents, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • const double *currents

      idsで指定されたID順に目標電流[mA]を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_GetPresentCurrent anchor.png[109]

指定IDの現在電流を取得する。

bool DXL_GetPresentCurrent (TDeviceID dvid, uint8_t id, double *current);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double *current

      取得した現在電流[mA]の保存先。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_GetPresentCurrents anchor.png[110]

複数IDの現在電流を取得する。
ライブラリ内のリストに登録されていないIDは無視される。

bool DXL_GetPresentCurrents (TDeviceID dvid, const uint8_t *ids, double *currents, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • bool *currents

      idsで指定されたID順に取得した現在電流[mA]を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top

PWM anchor.png[111]

現時点でXシリーズのみで使用でき、モデルが異なっていてもできる限り同じ運用ができるよう物理値のデューティー比[%]を用いてアクセスします。

Page Top
DXL_SetGoalPWM anchor.png[112]

指定IDへ目標PWMを指令する。
動作モードがPWM Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalPWM (TDeviceID dvid, uint8_t id, double pwm);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double pwm

      目標PWM[%]。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

  • 使用例
    #include <dx2lib.h>
    
    void main (void) {
      TDeviceID dev = DX2_OpenPort ("\\\\.\\COM10", 57600);
      DXL_GetModelInfo (dev, 1);
      DXL_SetOperatingMode (dev, 1, 16);
      DXL_SetTorqueEnable (dev, 1, true);
      // PWM指令 50%
      DXL_SetGoalPWM (dev, 1, 50.0);
      Sleep (1000);
      // PWM指令 -40%
      DXL_SetGoalPWM (dev, 1, -40.0);
      Sleep (1000);
      DXL_SetTorqueEnable (dev, 1, false);
      DX2_ClosePort (dev);
    }
    
Page Top
DXL_SetGoalPWMs anchor.png[113]

複数IDへ個別の目標PWMを指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPWM Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。

bool DXL_SetGoalPWMs (TDeviceID dvid, const uint8_t *ids, const double *pwms, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • const double *pwms

      idsで指定されたID順に目標PWM[%]を保持する配列の保存先。

    • int num

      対象とするID数

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_GetPresentPWM anchor.png[114]

指定IDの現在PWMを取得する。

bool DXL_GetPresentPWM (TDeviceID dvid, uint8_t id, double *pwm);
  • パラメータ
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t id

      対象とするID (0~252)。

    • double *pwm

      取得した現在PWM[%]の保存先。

  • 戻り値
    • bool

      処理が正常終了したらtrue、それ以外はfalseを返す。

Page Top
DXL_GetPresentPWMs anchor.png[115]

複数IDの現在PWMを取得する。
ライブラリ内のリストに登録されていないIDは無視される。

bool DXL_GetPresentPWMs (TDeviceID dvid, const uint8_t *ids, double *pwms, int num);
    • TDeviceID dvid

      DX?_OpenPortで開いた際のTDeviceID。

    • uint8_t *ids

      対象とするID一覧を保持する配列の保存先。

    • bool *pwms

      idsで指定されたID順に取得した現在PWM[%]を保持する配列の保存先。

    • int num

      対象とするID数

Page Top

オリジナルな定義 anchor.png[116]

TDXL_DevType
enum {
  devtNONE, devtDX, devtAX, devtRX, devtEX, devtMX, devtXL320, devtPRO, devtPROP, devtX
}
デバイスのグループ分け。Dynamixelはこのグループで概ねの機能が大別される。

TDXL_ModelInfo, *PDXL_ModelInfo
struct {
  uint16_t modelno;&brt// デバイス固有のモデル番号
  char name[16]; // デバイス名
  TDXL_DevType devtype; // デバイスのグループ
  struct { // 位置の範囲
    int32_t max;
    int32_t min;
  } positionlimit;
  struct { // 角度の範囲
    double max;
    double min;
  } anglelimit;
  struct { // 速度の範囲
    int32_t max;
    int32_t min;
  } velocitylimit;
  struct { // PWMの範囲
    int32_t max;
    int32_t min;
  } pwmlimit;
  double velocityratio; // 角速度変換係数 [deg/sec]
  double currentratio; // 電流変換係数 [mA]
  double pwmratio; // PWM変換係数 [%]
}
Dynamixel固有の情報(モデル名や本APIで使用する各制御値の物理値変換係数等)をまとめた構造体でアライメントは1バイト。
TAngleVelocity, *PAngleVelocity
struct {
  double angle;
  double velocity;
}
角度と各速度のペア。

Last-modified: 2019-03-27 (Wed) 15:58:32 (JST) (113d) by takaboo