C#講座 RS405CBのデータを読む

お知らせ

7月下旬なのに、このところ、涼しい日々が続きますね。
夏と言えば、土用の丑、皆さん、うなぎ食べましたか?

バレンタインデーと同じく平賀源内が夏場に売上が落ちるうなぎ屋に頼まれて作ったキャッチコピーと言われる「土用の丑の日はうなぎ」ですが、うまいこと乗せられてると言うか。w

私は、絶滅危惧種食べたとか言われながら、おいしくいただきました。
もう無くなると聞くと食べたくなるかんじとでもいうのでしょうか。
私はクジラ世代ではないですが、きっとクジラが食卓に上らなくなる直前もこんなかんじだったのかな
と思いをはせてみました。

うなぎはまだ生態が完全には解明されてない不思議な魚なのですよね。

RS405CBからのデータをもらう
ロボットサーボのいいところは、何と言っても、サーボの状態を取得できること。
これに尽きます。

モータの角度を指令するだけなら、RCサーボでもできたのですが、これを返せるようにしたのが近藤科学でした。
正直、あの時はほんとにいいサーボが出たなぁと思いましたし、今では当たり前ですが、当時は革命的な出来事ですね。
近藤科学がやってから続々と他のメーカーが続いたので、皆さん、近藤科学に感謝してください。w
(ほんとはこの連載でも近藤科学のシリアルサーボを最初に解説しようとおもってたのですが、B3Mのサンプルがないのだ。)

今回は、RichtextにRS405CBの返り値を表示してみます。
まずは返り値とは何なのかを見て行きましょう。
RS405CBではリターンパケットと呼ばれています。

ROBOTISサーボでは何も指定しなくても返り値が来るのですが、RS405CBは返り値くださいな、と言うと、指定した返り値をくれる形式になっています。
ではどう指定するのかを見て行きましょう。

例によってFutabaのRS405CBのマニュアルの20ページを見てください。

ここに詳しくリターンパケットを生成するときのお作法が載っています。
パケットを送るときに、リターンパケットをどのように返すのかを指定するので、マニュアルに従ってFlag(3番目のバイト)に返り値をどのように返すかというのを指定します。

とはいえ、これだけだとわかりにくいので、前回の連載でも使ったチェックサムの例題のパケットのFlagのところを見てください。
前回はFlagは0で送信していたので、実はリターンパケットなしです。

20130717blog2

この00になっているFlagのところを下の表(マニュアルより抜粋)のようにビットを計算して設定することになります。

20130724blog001

1バイトは8ビットですから上位4ビットは書き込みとかの指令、下位4ビットにビットを立てる(1にするの意味)と何かのリターンパケットが返ってくるようです。上位4ビットは今回使わないので、下位4ビットだけに的をしぼるのですが、ここはちょっと気をつけないと間違いそうですね。下の表(マニュアルから抜粋)を見てください。

20130724blog002

これは下位の4ビットだけを示した表です。
命令が届いたかどうかだけを見るのであれば、ACK/NACKの返信を要求でよいでしょう。
ROBOTISのように明示的に返り値をもらうためには、下位4ビットを1111となりますから、1バイトの中身は00001111となり、Flagは16進数で0FHになります。

これをパケットで書くと下の例題のようになります。
20130724blog003

もう一回マニュアルに戻って、FutabaのRS405CBのマニュアルの34ページを見てください。

メモリマップのNo.42(2AH)、No.43(2BH)は、サーボの現在位置が入っています。
34ページの例題では、Flag=09Hですから、下位4ビットに直すと1001になります。
Table4.3の表を見ると、1001はNo.42~59のデータを返すとあります。

34ページのままやると、欲しいデータだけでなくたくさんデータがもらえます。
1個、2個のサーボではそれでもよいですが、複数台あるような場合はトラフィックが増えるので、ピンポイントで狙った返り値だけがほしいですよね。

マニュアルには、フラグだけを送信する場合は【Address】=0、【Length】=0 、【Count】=1、【Data】は無しにと書いてあります。(【Count】の次にIDからCountまでのチェックサムが入ります)。
今回は狙った値だけがほしいので、AdrとLenを指定する必要があります。

上記の例の図では、AdrとLenも送っているので、Cntは0です。つまりFlagだけを送るのではなくて、このAdrからの長さのデータが欲しいよ、と指令しています。

これをパケットにして返り値を見てみましょう。
とやりたいところですが、データを受信するようにプログラムを変更する必要があります。
まずはその設定です。

前回作ったプログラムを出してください。
ここで、SerialPort1のイベントを設定します。
① デザインを出してください。下のほうに「SerialPort1」という表記がありますのでそこをクリックしてプロパティを出します。
② 稲妻マークを押してイベント設定にします。
③ DataReceivedイベントをダブルクリックして空のイベントを作成します。

20130724blog004
ここに出来た空の関数と下記の関数を次のように書いてください。

[css]
private void serialPort1_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
byte[] readData = new byte[serialPort1.BytesToRead];

//シリアルポートを受信
serialPort1.Read(readData, 0, readData.Length);

//delegateを呼び出す
Invoke(new AppendTextDelegate(packet_restortion), readData);
}

//delegate(処理を委譲)
delegate void AppendTextDelegate(byte[] text);

//委譲先の関数
private void packet_restortion(byte[] data)
{
//この関数には他にも処理を書きたいのでいったん別の関数に飛ばす
byte_to_hex_show(data);
}

//byteのデータを人間に読めるアスキー(16進)に変換
private void byte_to_hex_show(byte[] data)
{
string str = "";
string hex = "";

for (int i = 0; i < data.Length; i++)
{
hex = data[i].ToString("X");
if (hex.Length % 2 == 1)
{
hex = "0" + hex; //文字列が1個だったら0を追加
}
str += hex + " ";
hex = "";
}
//RichTextBoxの一番上に表示されるように設定
richTextBox1.Text = str + "\n" + richTextBox1.Text;
}
[/css]

これで準備が出来ました。

ではパケットを送るためにボタンを一つつけましょう。
下の図のようにREAD POSというボタンをつけます。

20130724blog005

ここでボタンをダブルクリックして、イベントを記述します。

[css]
private void button3_Click(object sender, EventArgs e)
{
int i;
byte[] com = new byte[8];
i = 0;

com[i++] = 0xFA; //Header
com[i++] = 0xAF; //Header
com[i++] = 0x01; //ID
com[i++] = 0x0F; //Flag
com[i++] = 0x2A; //Current pos address
com[i++] = 0x02; //length
com[i++] = 0x0; //count
com[i++] = get_check_sum(com, i);

if (serialPort1.IsOpen == true)
{
serialPort1.Write(com, 0, i);
}
}
[/css]

これで表示のところまでができます。
F5を押してコンパイルして挙動を見てください。

で、なんで公開が木曜の朝になったかと言うと。
ぐぬぬ、なぜか動かない。プログラムは間違ってないはず。
とちょっとあせったのをここでばらしてしまおう。

調べてみたらサーボのほうのIDが32になっていたのです。
そうなんです。
RIC90の予備品のRS405CBを使ってたのでIDが書きかわっていたのを忘れてました。(^^;

昨日はそれに気づかず、ふてくされて寝てしまったのでした。
でも、夢の中で、「IDちがってるんじゃないの?」というお告げがあり、朝、試したらID=32だったのでした。

それは動かないはずですね。
ID=1でプログラムしているのですから、あせらずにRS405CBのほうをID=1に直して再度チャレンジ。

20130724blog006

READ POSボタンを押すと、返り値が表示されました。

FD DF 01 00 2A 02 01 03 00 2B

これを読み解きましょう。

パケットの返り値は、FutabaのRS405CBのマニュアルの23ページを見てください。

返り値のヘッダーはFDDFとあります。
最初のFD DFで、あってますね。

サーボのID=01、あってますね。

次、Flag=00は、エラーなしを示してますね。よしよし。

Addressにあたるのは、2A、これは10進数だとNo.42、送った指令も42ですからあってますね。

その次、Lengthが02ということは、2バイト読んできたよと言う意味ですね。読みたいバイト数は2ですからこれもOKです。

次はCount、これはサーボの数ですからリターンパケットでは常に1、1個のサーボですからこれもあってますね。

その次はDataです。
マニュアルにはなぜかDataが書かれていません。

でも、返り値には03 00がセットされています。
ここでは2バイト分読んでくることになるわけですが、前の連載でも書いたとおり、Futabaサーボは上位、下位バイトが入れ替わっています。
つまり、データとしては、00 03ということになりますので、角度は0.3度ですね。
画像のTrackbarを見てもわかるとおり、場所はほぼ中点ですからあってます。

残りの2Bはチェックサムですから、XORで計算したらいいですが、ここでは飛ばしましょう。

というわけで、これで返り値も見られるようになりました。
RS405CBも電流値を取得できますので、擬似的にトルク制御っぽいこともできます。(数が多くなると返り値をもらうスピードがたりませんので何か工夫してください。)

シリアル通信講座、ROBOTIS、Futabaとやってきましたが、いかがでしたでしょうか?

どちらもパケットの作り方の基本、データ受信の基本だけの連載でしたので、もっとここ知りたかったのにというのがあるかもしれません。

これでFutabaの連載も終わりでちょっとさみしいです。
これで連載も終わりなので、今後の展開はないですが、各自応用してみてください。

たとえば、解説しなかったロングパケットもまた使い勝手いいので、マニュアルを見ながら、自分だけのモーションエディタなども作ってみるのも楽しいと思います。
私はFutabaのメモリマップエディタが便利だったので、ROBOTIS用のメモリマップエディタを自分で作りました。

ちなみにROBOTISもFutabaも返り値が長くなるとシリアルポートが受信しきれずにパケットが千切れることがあります。
こういう千切れたパケットの復元アルゴリズムを考えるなども萌えでした。

個人的にはどちらも使いやすいとはおもっていますが、好みがあるとおもいますので、お好きなほうを利用されるのがいいとおもいます。

では、皆さん、いろいろ試してみてください!
ぜひサーボはアールティで購入を。w

【今回使う品物のリスト】


RSC-U485

RS40X用サーボアーム

バッテリーPR-4S780P

RS405CB

タイトルとURLをコピーしました