• カテゴリ Dynamixel の最新配信
  • RSS
  • RDF
  • ATOM

ブログ - Dynamixelカテゴリのエントリ

ArduinoでDynamixelを動かす

カテゴリ : 
Dynamixel
2015-11-9 9:20
結構な頻度でArduinoを使ってDynamixelを制御したいといった問い合わせをいただきます。

ソフトを横着するかハードを横着するかで選択肢が分かれますが、AX-12AであればとりあえずArduinoのなにがしかの端子をSIGNAL端子に直結し電源を供給するだけで接続はおしまいだったりします。
そこまで簡易的だと通信プロトコルの生成以外の部分でプログラムを頑張らないといけませんが、その覚悟があれば最短で接続は終わります。

技術サポートではそういった無粋な回答で終わっているかも知れず、その場合はごめんなさい。

片やソフトをとっても横着したい場合はとなると、そこそこ回路でフォローしないといけないかと思います。
以前ブログにATmega128マイコンボードとAX-12を接続する方法を紹介しましたが、概ねこの様な回路があればソフト側で対処する部分は最小になる筈です。この回路では半二重通信であるが故のバッファのイネーブル信号をソフト的に生成しないといけない事になっていますが、送信に連動してイネーブル信号が切り替われば良いと割り切ればさらにソフトは簡単になります。
といったところで、バッファICが1個、インバータ1個、抵抗数本で何とかする回路を描いてみました。それでも部品点数が多と思うかも知れませんが、ソフトをめいっぱい横着するにはこのぐらい無いといけないかと。

TX/RXとあるラベルの信号線は、各々Arduinoのシリアル用送信/受信端子、それ以外は電源をつなぎます。IC類は回路図を描く際に手持ちのライブラリから引用しただけなので、互換のもので代替して構いません。また、保護やら何やら必要に応じて部品を付加するのはもちろんOKです。
ちなみに、RS485 I/Fの場合はバッファICをRS485のトランシーバに変更するだけです。


ここまで用意すれば、ソフトではDynamixelのプロトコルを生成するのと送受信に注力できます。

Arduinoにも種類がありますが、ここではArduino UNOを前提に記述します。UNOにはプログラムの書き込みやデバッグに使用するUSBポートが備わっていますが、その機能につなっているTX/RX端子を本回路のTX/RXにつないでしまうと、唯一の通信ポートが占有されしまいます。ここではSoftwareSerialのライブラリを使用して他の端子に割り当て直す事としました。
仮に8pinをRX、9pinをTXとしてSoftwareSerialの端子の割り当てた場合は以下の宣言をしておきます。
#include <SoftwareSerial.h>

SoftwareSerial dxif(8, 9); // pin8 is RX, pin9 is TX
初期化部ではボーレートと後々受信処理を楽にするためのタイムアウトの設定をしておきます。SoftwareSerialは高いボーレートに対応しないため、今のうちにAX-12Aのボーレートを57143bpsに変更しておきましょう。
void setup() {
dxif.begin (57143);
dxif.setTimeout (50);
}
これでSoftwareSerialで通信するための下処理は終わりです。

では早速AX-12Aを動かしてみますが、簡単に1秒間隔で往復運動をさせてみましょう。
まず位置指令を行うsendpos関数を作り、idと位置を引数としました。ここではGoalPositionへ2バイトの位置情報を書き込めば良いため、インストラクションパケットのidとpos、チェックサム以外は固定とし、直接固定値で配列内に記述しています。
このパケットを受け取ったAX-12Aは6バイトのステータスパットを返送してきますので、6バイト受信した後にその内容を簡易的に確認した結果を返値にしています。もしAX-12Aからの返信が無かったとしても、初期化時に設定したタイムアウト時間を経過した後に抜けます。
あとはloopの中でsendposを呼び出すだけです。
bool sendpos(uint8_t id, uint16_t pos) {  
uint8_t buf[9] = {
0xff, 0xff, // header
id, // id
5, // inst
3, // size
30, // address
(uint8_t)(pos & 0xff), (uint8_t)(pos >> 8),
0
};
for (int i = 2; i < 8; i++) buf[8] += buf[i]; // calc sum
buf[8] = ~buf[8];
while (dxif.available ()) dxif.read (); // clear buffer
dxif.write (buf, 9); // send inst packet
if (dxif.readBytes (buf, 6) == 6) // read stat packet
return (buf[2] == id) && (buf[3] == 2) &&(buf[4] == 0);
return false;
}

void loop() {
sendpos (1, 0); // Set Goal Position to 0
delay (1000); // Delay 1 sec
sendpos (1, 1023); // Set Goal Position to 1023
delay (1000); // Delay 1 sec
}
ハードさえちょっと頑張れば、パケットの生成とその送受信以外に意識するところはありません。いかがだったでしょうか。
ArduinoでDynamixelをコントロールするのを躊躇していたのでしたら、参考にしていただければと。あ、他にもFUTABA社KO社のシリアルI/Fを持ったサーボモータでも同様の回路で動かせます。

P.S.
Arduinoに乗っけてすぐに使えるDynamixel shieldの要望があれば作っても良いかなと。


技術
PROシリーズはMXシリーズやAXシリーズとは異なる通信プロトコル(V2.0)で運用され、従来(V1.0)との相互互換性は無い。また、XL-320もPROシリーズと同類だが、完全互換でもない。
V1.0 Status Packet

V2.0 Status Packet


PROシリーズは扱うアドレスやデータのサイズが大きくなっている都合、自ずとパケットの構成要素のサイズが大きい。
そういった面からV2.0ではチェックサムはCRCになり、最大通信速度が10Mbpsに上がっているので、ハード同様に多少なりとも堅牢で応答性の良い環境にはなっている。

それとは別に、V2.0の通信プロトコルにおいて今まで指摘されなかった事を良い事に、仕様と実態が異なる事をライブラリ内でクローズしていた。
実際にはステータスパケット中のErrorはV1.0と同じ挙動をしているが、本来はそれとは異なるものとしている。

プロトコルの処理を独自に組み込んで使用する際は、現時点では「とりあえず」ErrorはV1.0と同様に各要因がビットアサインされているものとして処理してもらう他ない。
最終的にどのように修正されるかは流動的だが、V2.0の仕様に合わせたファームウェアの改修で対処する方針の様なので、いずれ変更せざるを得ない事をお忘れ無きよう。

技術

Status Return Levelの復帰

カテゴリ : 
Dynamixel
2014-8-26 10:30
Dynamixelはシリアル通信でホストとコミュニケーションを行いますが、そのコミュニケーションを設定によって容易に阻害する事ができてしまいます。そのアイテムはStatus Return Levelでして、デフォルトは2なのですが、0にすると通常のコミュニケーションは一切できない様に見せかけられてしまいます(唯一Pingパケットには応答します)。
この設定は不揮発領域に配置されており、電源を入れ直しても自動的に元に戻る事はありません

ということで、アクチュエータ系のDynamixel前提ですが、Status Returl Levelのリカバリ用プログラムを作ってみました。
ツクリは単純でして、ボーレートを順次変えながらPingで検索し、見つかったidに対してReadが失敗したらStatus Return Levelが0になっていると仮定し、強制的に2で上書きするといったものです。

#include <stdio.h>
#include <conio.h>
#include "dxlib.h"

#define OFFSETTIME  (50)  // タイムアウトオフセット[ms]

void main (void) {
  TDeviceID   dev;
  char        inputcomport[10], mycomport[20]="\\\\.\\";
  int         bdiv=-1, baud, i, yn;
  uint16_t    val16;
  uint8_t     val8;

 printf ("input com name=");
  gets (inputcomport);
  printf ("use [%s]\n",inputcomport);
  strcat (mycomport, inputcomport);

  if ((dev = DX_OpenPort (mycomport, 3000000))) {
    DX_SetTimeOutOffset (dev, OFFSETTIME);
    printf ("Open success\n");
    for (bdiv=-1;bdiv<250;bdiv++) {
      // ボーレートの変更
      if (bdiv >= 0) DX_SetBaudrate (dev, (baud=2000000 / (bdiv + 1)));
      else           DX_SetBaudrate (dev, (baud=3000000));
      printf("\rbaud=%7d ",baud);

      uint32_t num = 253;
      TDxAlarmStatus stat[255];
      // Ping2で検索
      if (DX_Ping2 (dev, &num, stat, NULL)) {
        printf ("%d device found\n", num);
        // 検索で見つかったデバイスを列挙
        for (i = 0; i < num; i++) {
          // アドレス16(Status Return Level)を読み出し
          if (DX_ReadWordData (dev, stat[i].id, 0, &val16, NULL)) {
            printf ("Found ID=%d stat:$%04X modelno:$%04X\n", stat[i].id, stat[i].Status, val16);
          } else {
            // 読み出しに失敗したらおそらくStatus Return Levelが0だろう
            printf ("Found ID=%d stat:$%04X modelno:???? \n", stat[i].id, stat[i].Status);
            printf ("Do you want to reset Status Return Level ? (y/n)->");
            yn = getch ();
            putch( yn);
            if (yn == 'y' || yn == 'Y') {
              printf ("\nupdate....");
              DX_WriteByteData (dev, stat[i].id, 16, 2, NULL);
              Sleep (800);
              if (DX_ReadByteData (dev, stat[i].id, 16, &val8, NULL)) {
                if (val8 == 2) printf ("success\n"); else printf("fail\n");
              } else printf ("fail\n");
            } printf ("\n");
          }
        }
      }
    }
    DX_ClosePort (dev);
  } else {
    printf ("Open error\n");
  }
}
DXLIBを使っていますので、コンパイルにはGCC Developer Liteを使用したコンパイルの方法を参考にしてください。

また、コンパイルが面倒といった場合は、ちょっとイリガルな方法ですがDXCONFを使って書き換える事もできます。
Status Return Levelが0でもPingには応答するため、スキャンすると以下の様に???(不明なデバイス)として列挙されます。


通常であれば、該当デバイスをクリックする事で右半分に表示されるコントロールテーブルがそのデバイスに応じて切り替わるのですが、不明なデバイスの場合はこの画面キャプチャの様に5個のアイテムのみの表示になってしまい、値自体も不定となります。
ではこのコントロールテーブル上にマウスカーソルを置き、右クリックしてください。ポップアップメニューが表示されますので、もしデバイスの型式がわかっているのであれば、この中から該当するデバイスを選択します。


選択したデバイスに応じた コントロールテーブルの表示に切り替わりますので、その後強制的にNo.16 Status Return Levelを2に変更します。処置後は全ての通信ができるようになる筈です。


お試しください。

技術

DXLIB V3.0公開

カテゴリ : 
Dynamixel
2014-6-11 15:06
Dynamixel Library Version 3.0を公開しました。

既にいくつかのソフトウェアには導入済みですが、DXLIB2(XLやProシリーズの新プロトコル用API)の機能拡張に伴い、パラメータの互換性を保つためDXLIBも修正を施しました。
主な変更点は以下の通りです。
  • アドレス指定を8bitから16bitに変更
  • 不明瞭だったTDxAlarmStatus構造体のアライメントを1バイト境界に固定
  • 似非ReadSync APIを追加
  • 不定なDevice IDを指定された際のクラッシュを簡易的にブロック
  • dllのソースをC++に
  • API引数の変更に伴い各サンプルを修正
  • LabVIEWのサンプルを大幅に拡充
  • EXCELのVBAを64bitに対応
特にLabVIEWから制御する際の使い勝手を向上させたのと、サンプルプログラムを増やしました。特定のアイテムへのアクセスを対象としたviは、ループ中に適用しても不用意にトラフィックを上げない様な仕掛けや、LabVIEWのイベントストラクチャを多用したり、ダイナミックなポートの開閉等、そこそこがんばってみました。

PCから直接制御する際の参考にして下さい。

技術

DynamixelをPCから直接コントロール

カテゴリ : 
Dynamixel
2014-6-2 15:00
■Dynamixelをコントロールするにあたり

DynamixelシリーズをPCから制御ためのAPIとしてDXLIBを公開していますが、Dynamixelが持っているコントロールテーブルへアクセスする事に特化しているため、Dynamixelの素性に応じた違いや行いたい制御方法によっては無限とも言える組み合わせでユーザ自ら試行錯誤してプログラミングしなくてはなりません
一応1台のDynamixelの動作を確認したり設定を変更するのであればDXCONFというツールである程度対応できますが、こちらもまたコントロールテーブルの中身を可視化させて書き換えがしやすくなっている意外には特別な機能は持っていません。
さらに、マイコンから制御するとなると、これまたPCとは違う知識が要求される面が多々あって、「ひとまず動かしてみる」といった用途には向きません。

結局のところ、プログラミングスキルを身につけないと、要求仕様のレベルが少し上がるだけでハードルがうんと高くなってしまうのが実情です

■とりあえずでもある程度動かす

Dynamixelを使用する台数が多いアプリケーションは用途が複雑なケースが大半でしょうから、双方向で通信できる事に期待した機能も盛り込みたくなるのが心情です。しかしながら構成が変わる度にゼロからプログラミングするのはかなり酷ですし、複雑であるがゆえに初期の段階で試運転したくなります。

ということで、少しでもそれらのニーズの一助となればと思い、PCとDynamixel間をDXHUBUSB-3WAY等で接続した環境において、予め設定しておいた様々な動きを再現させるだけのツールを準備する事としました。
予め各々異なるIDを振りボーレートを同じにしたDynamixelを、みんなつないで一斉に面倒を見てくれるといった感じです。
実は、これと同様な環境がROBO-ONEというイベントで頒布したツールにも含まれていましたが、イベントという事もあり解る方にしかわからない環境を要求されてしまうのと複数メーカをサポートしているため、二次的に頒布するには使い勝手が悪いのは否めませんでした。

今回は用途を「ひとまず複数台を連動してスムーズに動かす」に限定し、全てNI社LabVIEWを使って作り直したものを提供することにします。

■大まかな構成

プログラムは2つ提供します。

<ホスト>
Dynamixelとの通信を高い頻度で行い続けるホスト役で、ウィンドウの外観からは動いている事をうかがい知ることがほとんどできませんが、Dynamixelならではの処理ルーチンが集約されています。
基本的に、後述する他のプログラム等から角度と角速度程度の情報をこのホストに投げかけると、目的の角度まで自動的にスムーズに動かしてくれます。
また、アラームやエラー・温度・トルク・現在角度をリアルタイムに取得し続けていますので、これらの情報を他のプログラムからモニタする事もできます。
なお、DynamixelのID・Baudrate・Return Delay Timeの3つの設定に限り本ツールより直接変更する機能も盛り込んでいますので、よほどのことが無い限りDXCONFいらずです。

基本的な操作は、IDやDynamixelの種類、Goal Position等の位置と角度(deg)の変換係数、ホームポジションといった情報を登録するだけですが、接続されたDynamixelを自動的に検索して登録させる事もできます。



<エディタ>
DynamixelのネイティブなIDや位置の概念は無く、ホストに登録された順に軸を扱い、各軸のホームポジションからの角度(deg)の偏差と時間を扱うポーズエディタです。
アニメーションの様にある時点の角度を記憶させ、それを複数用意し、最終的に連続して再生するといった手法を採用しています。本当のアニメーションの様にたくさんのポーズを作る必要はなく、今のポーズから次のポーズへ移行する時間を設定する事で、その間は自動的に直線的に補完します。ようするに道のりを問わないのであれば、A地点からB地点移動する場合はA地点とB地点の2つのポーズだけ登録し、その間を移動する時間を設定すれば良い訳です。なお、FDIII-HCのライブラリとほぼ考え方は一緒です。
ポーズはファイルとして保存できますので、二次利用も容易なはずです。

なお、本ツールではポーズの編集と再生以外の機能は持たせるつもりは無く、さらなる利便性やニーズに応える機能は別の機会に提供しようと考えています。
一応ホストはそのままで、エディタをIKを使った6軸アームロボット制御プログラムに入れ替えたものや、もっと実機に近いイメージでポーズをデザインできるエディタ等は既に一部に提供済だったりしますが、他に何か楽しげなネタがあればリクエストいただければと思います。



■制約等

<PC>
最近のマルチコアCPUを搭載したPCがあると良いです。必須というわけではありませんが、ある程のリアルタイム性を確保するためには、ここ数年来のPCがあると良いです。

<レイテンシ>
DXHUBやUSB-3WAYはPCのUSBポートに接続して使用しますが、これらに適用されるデバイスドライバをデフォルトのまま使用すると通信の応答性が極端に悪くなり、スムーズな動作ができなくなります。
各I/Fのドキュメントに記載されている応答の待ち時間を1にしなくてはなりません。

<ボーレート>
先のレイテンシと似ていますが、通信速度が遅いと情報の伝達速度も遅くなります。できるだけ高速な通信が望まれるため、ボーレートは1Mbpsを推奨します。

<Dynamixel I/F>
よく見受けられる散発的だったり一方通行のDynamixelとの通信プログラムとは異なり、ホストではかなり込み入ったトランザクションを処理します。そのため、正常に通信が行えないDynamixelがI/F上に1台でも存在していてはなりません。

<ネットワーク環境>
LabVIEWの機能をふんだんに使っているため、オンライン・オフラインにかかわらずファイアウォールの影響を受けます。アンチウィルス等でポート開放の動作が阻害されないようにしなくてはなりません。

<LabVIEWのランタイムエンジン>
提供するプログラムはexeファイルではありますが、LabVIEW 2011SP1にて開発されているため、ランタイムエンジンを予めインストールしておく必要があります。

<制限>
フリー版と有償版を準備する予定ですが、どの程度の機能制限をするかは検討中でして、今のところ軸数や動作時間を予定しています。



といったところで、今月中には試食版を提供できる予定です。

技術