コマンド方式サーボKRS-6003HVの制御　その1

mbed

2012/11/25時点で以下のコードはそのままでは利用できないようです。以下の記事をご確認ください。


mbedでSerialHalfDuplexが使えなくなったので古いリビジョンのmbedライブラリを使ってみる
http://d.hatena.ne.jp/rinie/20121125/1353770901


近藤科学のコマンド方式サーボKRS-6003HVの動作テストをしてみたのでその備忘録です。


データシートが手元にないので近藤科学が公開している情報が頼りになりました。制御方法はホームページに全て掲載されています。

※上記ページでは一部誤表記*1があるようです。


PWMではなくシリアル通信で制御するサーボを扱うのは初めてなので分からないことばかりでしたが少しづつ読み進めてみました。
まとめとしては、

  • 115200bpsの半二重シリアル通信で制御（ボーレートは変更可）
  • パリティビットはeven、その他は通常のシリアル通信と同じようにデータ長8bitやストップビット1bitの設定でOK
  • サーボ1つ1つにIDが振られている
  • マイコン側から送るコマンドはだいたい3〜4byte
  • サーボからは送ったコマンドのループバック＋αが返ってくる
  • サーボの位置情報、温度、電流量等も取得可能
  • 電源は9〜12V


fig1:サーボ本体とコネクタ配置
コネクタは2つありますがどちらに接続しても問題ない様子。


テストするためにmbedを使って制御してみることにしてみました。シリアルサーボ制御方法（１）　回路編を参考に回路を組んでプログラムを作成。mbedにSerialHalfDuplexクラスが予め用意されていたのでプログラムの実装は簡単でした。電源には定電圧源から10Vを印加、信号レベルは5Vです。


fig2:実験回路


プログラム

#include "mbed.h"

Serial pc(USBTX, USBRX);
SerialHalfDuplex master(p9, p10);

int main() {
    master.baud(115200);
    master.format(8, Serial::Even, 1);      // data length = 8bit, parity = even, stop bit = 1bit

    // read ID
    //master.putc(0xFF);
    //master.putc(0x00);
    //master.putc(0x00);
    //master.putc(0x00);
    //int id = master.getc();
    //pc.printf("ret : %d\n", id);
    
    // set position
    master.putc(0x82);          // ID = 2
    master.putc(0x3A);
    master.putc(0x4C);
    int ret1 = master.getc();
    int ret2 = master.getc();
    int ret3 = master.getc();
    pc.printf("ret : %d, %d, %d\n", ret1, ret2, ret3);
    
    while(1);
}

コメントアウトした10〜16行目でサーボのIDが取得できます。その結果このサーボのIDは2でした。
18行目以降で位置の変更です。このサーボは7500をニュートラルとして3500〜11500で位置が移動できるようなのでここでは7500に移動しています。位置変更は指示コマンド＋14bitの位置情報を3byteで送ります。
19行目は位置変更コマンド0b100にIDの0b00010を合わせたものです。0b10000010=0x82
20行目と21行目は7500=0b01110101001100を7bitずつに分けてMSBに0を付加したものです（指示コマンド以外はMSBを0にする必要があります）。すると2byte目は0b00111010=0x3A、3byte目は0b01001100=0x4Cになります。


以上で動作させたところ、mbedのリセットによりサーボにコマンドが送信されると直ちにサーボが動作してある位置まで移動して静止しました。60kg/cm超のトルクがあるのでさすがの保持力です。
この時のサーボから返ってきた値は以下のようになりました。

ret : 130, 78, 25(1回目のリセット。この表示と同時にサーボが動作)
ret : 130, 58, 76(2回目のリセット。サーボは所定の位置で静止したまま)
ret : 130, 58, 76(3回目のリセット。サーボは所定の位置で静止したまま)
ret : 130, 58, 76(4回目のリセット。サーボは所定の位置で静止したまま)

ここで130=0x82は送ったコマンドそのもの、1回目リセット後の2つ目と3つ目の返り値78=0x4Eと25=0x19は移動前の位置情報だと思われます。2回目のリセット後ではその部分が58=0x3A、76=0x4Cに変わっています。


とりあえず簡単ではありますがコマンド方式のサーボを動作させることができました。従来のPWMによる制御よりも複雑ですが多くのメリットがありそうなので今後さらに勉強してみたいと思います。

*1:シリアルサーボ制御方法（２）　ICS編の"ID番号を読み出す"内で「R_CMDには、ID読み込みコマンド0b111XXXXXとサーボモーターのID番号0b000XXXXXが合わさった１バイトのデータで、MSBが０になったものが返ります。例えば読み出したサーボモーターのID番号が25（0b000110011）だった場合は、R_CMD=0b011110011（243＝0xF3）が返ってきます。」と、赤で示した余分な文字列が含まれています。