今日はテクノフロンティアに行ってきたyukiです。
最近は出かけることが多くてなかなかオフィスにはいません。
先週は出張で飛ばしてしまい、楽しみにしていた方々にはすみませんでした。
今週はパケット作りを完成させて、サーボを動かすところまでやりましょう。
今週は、RS405CBのパケット作りを完成させていきますので、まずは、前回の宣言どおり、チェックサムの計算から行きましょう。
なぜチェックサムから作るかというと、チェックサムはどんなコマンドでもパケットを作るのには汎用な関数なので、先に作っておくのが便利だからです。
私も慣れてない初期のころ、全部の関数でチェックサムを計算する手順を書いていたものです。でも、先人の知恵に倣って、ここは汎用なので関数化してしまうのがやはり得策でした。それに、関数として呼び出すほうが間違いもなくなるのでバグが減ります。
不具合が出たときに、疑う箇所は少なければ少ないほど効率的なプログラミングが出来ますから、ぜひ皆さんも参考にしてみてください。
さて、では、FutabaのRS405CBのマニュアルの17ページの下を見てください。
ここにチェックサムの計算方法が書いてあります。
「送信データの確認用のチェックサムで、パケットのIDからDataの末尾までを１バイトずつXORした値を指定します。」とあります。XORとは、入力のうち1つだけがHighのとき、Highを出力し、入力が両方Highまたは両方Lowのときは、Lowを出力する排他的論理和といわれる計算方法です。
Highとは01で言ったら、1です。Lowとは01で言ったら0のことです。
ビットで言うと下記のようになります。
XORの入力と出力をみてください。このようにそれぞれのビットをIDからデータ長までのバイトごとに演算していきます。
マニュアルには、例題もあって、次の送信データのチェックサムは、次のようになります。
Windowsをお使いの人は、電卓を関数電卓（プログラマモード）にして計算してみましょう。
計算には赤枠で出した16進を設定し、Xorボタンを使います。
最終的には1Cになりますね。
C#では、排他的論理和は”^”で表現します。
また、入ってくるであろうチェックサムを計算すべきコマンド列をbyte[] comということにして、intでbuf_sizeでcomの大きさを引数でもらうことにします。
追加するプログラムは、下記になります。
private byte get_check_sum(byte[] com, int buf_size)
{
byte check_sum = new byte();
int i;
check_sum = 0;
for (i = 2; i < buf_size - 1; i++)
{
check_sum = (byte)(check_sum ^ com[i]);
}
return check_sum;
}
では、これをC#で実装します。
前に作ったプログラムを出してください。
フォームの「デザイン」タブではなく、「Form1.cs*」タブを開いてください。
赤枠のあたりに先ほどのプログラムを追加します。
次に、Trackbarでサーボを動かすためにTrackbarの設定をしましょう。
今度は、「デザイン」タブでフォームデザインを表示します。
コマンド式の場合、RS405CBは、±150度の動作ができます。
そこで、図の赤枠のところでTrackbarの稼動範囲を次のように設定します。
① Maximamを3000にします。
② Valueを1500にします。（中点の計算で使うのと表示を真ん中にするため）
その下の図を見てください。Valueのずっと下のほうにTickfrequencyというのがあります。
③ 表示を見やすくするためにTickfrequencyを100にします。（もっと広げてもOK）
このようにして、Trackbarで角度を示せるようにします。
RS405CBは、0.1度単位まで指定ができるので、Trackbarでは0の桁をあげて計算させます。RS405CBは、一応、0.1度単位の指定ができますが、中に使っている角度センサはポテンショメータなので、それほど正確ではありません。大体精度は約1度くらいだとおもうとよいでしょう。もちろん、研究者の言う0.01度の精度までは得られません。しかし、1度の精度はRIC90などのゆっくりとした2足歩行をさせる程度の要求事項であれば十分な精度です。
閑話休題。
次に、角度を指定してみましょう。
マニュアルには、
とあります。
つまり、プラスマイナスの記号をつけて計算した結果を上位下位のバイトを入れ替えて指定することになります。
これを実装してみましょう。
ここで、先程、Trackbarの最大値を3000にして、1500を中点(Valueに設定)したことを思い出してください。
Trackbarの現在値であるValueは1500を境にして、プラスとマイナス方向になります。
つまり、サーボの角度をdegreeとしたら、degree-1500でプラスマイナスが得られます。
このValueの値とマニュアルにプロトコルを実装してみましょう。
まず、稲妻マークの「デザイン」タブのほうで、ValueChangedイベントを登録します。
プログラム画面に切り替わりますね。
ここで、次のように入力します。
//data calculation
の次の2行が、Trackbarの値を代入しているところです。
private void trackBar1_ValueChanged(object sender, EventArgs e)
{
int buf_size = 10;
byte[] com = new byte[buf_size];
byte l_bit;
byte h_bit;
int degree = trackBar1.Value-1500;
//data calculation
h_bit = (byte)((degree & 0xFF00) >> 8);
l_bit = (byte)(degree & 0xFF);
com[0] = 0xFA; //Header
com[1] = 0xAF; //Header
com[2] = 0x01; //ID
com[3] = 0x00; //Flag
com[4] = 0x1E; //Address
com[5] = 0x02; //length
com[6] = 0x01; //count
com[7] = l_bit; //data lower
com[8] = h_bit; //data higher
com[9] = get_check_sum(com, buf_size);
if (serialPort1.IsOpen == true)
{
serialPort1.Write(com, 0, buf_size);
}
}
これで実装はできたのですが、うごかす前に、トルクONボタンをつけましょう。
Futabaのサーボは、トルクを明示的にON/OFFする必要があります。
「デザイン」タブに戻ってください。
ボタンをセットして、Textをトルクの”ON”にしておきましょう。
このボタン名でON/OFFを切り替えるようにボタンをダブルクリックしてプログラムを実装します。
Button2のtextがONかOFFかで挙動を変えます。
ONのときに、トルクをONにし、OFFのときにトルクをOFFにします。
次のように実装してください。
private void button2_Click(object sender, EventArgs e)
{
int i;
byte[] com = new byte[9];
i = 0;
com[i++] = 0xFA; //Header
com[i++] = 0xAF; //Header
com[i++] = 0x01; //ID
com[i++] = 0x0; //Flag
com[i++] = 0x24; //Torque map address
com[i++] = 0x01; //length
com[i++] = 0x01; //count
if (button2.Text.CompareTo("ON") == 0)
{
com[i++] = 0x01;
button2.Text = "OFF";
}
else
{
com[i++] = 0;
button2.Text = "ON";
}
com[i++] = get_check_sum(com, i);
if (serialPort1.IsOpen == true)
{
serialPort1.Write(com, 0, i);
}
}
このように、ONのときにトルクを入れたら、ボタン名を「OFF」に変えておきます。同様に、「OFF」になっているときは「ON」を表示するようにします。
このボタンを押すことでRS405CBのトルクのON/OFFができます。
さらに、このFormではシリアルポートを閉じる設定がないので、フォームを閉じるときにシリアルポートを閉じる設定をしましょう。そうしないと、PCを再起動するはめになります。
「デザイン」タブに戻ってください。
Form1を選び、稲妻マークのイベントを出します。
このイベントの中で、Formが閉じるときに呼ばれるイベントであるFormClosedをダブルクリックして空の関数を出します。
ここで、もし、serialPort1が開いていたら閉じるプログラムを書きます。
下記を追加してください。
private void Form1_FormClosed(object sender, FormClosedEventArgs e)
{
if (serialPort1.IsOpen == true)
{
serialPort1.Close();
}
}
これで、フォームが閉じるとき（プログラムの終了時）に自動的にシリアルポートが閉じられることになります。
さて、F5でコンパイルして、動かしてみましょう。
COMポートとBaudrateを選んで「Set」ボタンを押します。
シリアルポートがきちんと設定できていれば、このようなウィンドウがでますので、OKを押します。
エラーのときもエラーが出ますので適切なCOMポートとボーレートを選んでください。
早速、画像ではすでにOFFになっていますが、トルクをONにして、trackbarを動かしてサーボを動かしてみましょう。
すいすい動きますね。
モータのパワーがけっこうありますので、怪我しないように気をつけて動かしてみてください。
次回は、Richtextに返ってくるAckを表示したり、現在位置を取得して表示したりしましょう。
それでは！
【今回使う品物のリスト】
 RSC-U485 |
 RS40X用サーボアーム |
 バッテリーPR-4S780P |
 RS405CB |
