ブログ - Status Return Levelの復帰

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に変更します。処置後は全ての通信ができるようになる筈です。


お試しください。

技術

トラックバック

トラックバックpingアドレス http://www.besttechnology.co.jp/modules/d3blog/tb.php/161