C#講座 RS405CB動く!

お知らせ

今日はテクノフロンティアに行ってきたyukiです。
最近は出かけることが多くてなかなかオフィスにはいません。
先週は出張で飛ばしてしまい、楽しみにしていた方々にはすみませんでした。
今週はパケット作りを完成させて、サーボを動かすところまでやりましょう。

今週は、RS405CBのパケット作りを完成させていきますので、まずは、前回の宣言どおり、チェックサムの計算から行きましょう。
なぜチェックサムから作るかというと、チェックサムはどんなコマンドでもパケットを作るのには汎用な関数なので、先に作っておくのが便利だからです。

私も慣れてない初期のころ、全部の関数でチェックサムを計算する手順を書いていたものです。でも、先人の知恵に倣って、ここは汎用なので関数化してしまうのがやはり得策でした。それに、関数として呼び出すほうが間違いもなくなるのでバグが減ります。
不具合が出たときに、疑う箇所は少なければ少ないほど効率的なプログラミングが出来ますから、ぜひ皆さんも参考にしてみてください。

さて、では、FutabaのRS405CBのマニュアルの17ページの下を見てください。

ここにチェックサムの計算方法が書いてあります。

「送信データの確認用のチェックサムで、パケットのIDからDataの末尾までを1バイトずつXORした値を指定します。」とあります。XORとは、入力のうち1つだけがHighのとき、Highを出力し、入力が両方Highまたは両方Lowのときは、Lowを出力する排他的論理和といわれる計算方法です。

Highとは01で言ったら、1です。Lowとは01で言ったら0のことです。
ビットで言うと下記のようになります。

XORの入力と出力をみてください。このようにそれぞれのビットをIDからデータ長までのバイトごとに演算していきます。
20130717blogxor

マニュアルには、例題もあって、次の送信データのチェックサムは、次のようになります。
20130717blog2

Windowsをお使いの人は、電卓を関数電卓(プログラマモード)にして計算してみましょう。
計算には赤枠で出した16進を設定し、Xorボタンを使います。
最終的には1Cになりますね。

20130717blog3

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*」タブを開いてください。

20130717blog4
赤枠のあたりに先ほどのプログラムを追加します。

次に、Trackbarでサーボを動かすためにTrackbarの設定をしましょう。
今度は、「デザイン」タブでフォームデザインを表示します。

コマンド式の場合、RS405CBは、±150度の動作ができます。
そこで、図の赤枠のところでTrackbarの稼動範囲を次のように設定します。
① Maximamを3000にします。
② Valueを1500にします。(中点の計算で使うのと表示を真ん中にするため)
その下の図を見てください。Valueのずっと下のほうにTickfrequencyというのがあります。
③ 表示を見やすくするためにTickfrequencyを100にします。(もっと広げてもOK)

20130717blog5

20130717blog6

このようにして、Trackbarで角度を示せるようにします。
RS405CBは、0.1度単位まで指定ができるので、Trackbarでは0の桁をあげて計算させます。RS405CBは、一応、0.1度単位の指定ができますが、中に使っている角度センサはポテンショメータなので、それほど正確ではありません。大体精度は約1度くらいだとおもうとよいでしょう。もちろん、研究者の言う0.01度の精度までは得られません。しかし、1度の精度はRIC90などのゆっくりとした2足歩行をさせる程度の要求事項であれば十分な精度です。

コラム

角度を測るセンサとして、RS405CBに入っているのはポテンショメータと書きましたが、皆さんはポテンショメータって知っていましたか?

一般的に、角度を測るセンサにはポテンショメータやエンコーダがあります。
抵抗値で角度を検出するセンサのことをポテンショメータといいます。
RS405CBのカットモデルの写真があるので、赤丸の中を見てください。白い棒の先がポテンショメータのありかです。
(写真はFutabaのカットモデルを撮影させていただいたものです。)
20130717blog14

ポテンショメータは、サーボの角度検出やラジコンのプロポを分解するとレバーの角度を検出するために入っています。

FutabaのRS405CBをはじめとするロボット用サーボにはポテンショメータが入っています。ROBOTISのMXシリーズ以外もポテンショメータです。(MXシリーズはエンコーダ)

ポテンショメータを「ポテンションメータ」と書いているマニュアルや文献を時折見かけます。しかし、「ポテンショメータ」が正しい表記です。英語ではPotentiometerと書きますので、どこにも「ン」にあたるnは入らないのです。

閑話休題。
次に、角度を指定してみましょう。
マニュアルには、

20130717blog7
とあります。
つまり、プラスマイナスの記号をつけて計算した結果を上位下位のバイトを入れ替えて指定することになります。
これを実装してみましょう。

ここで、先程、Trackbarの最大値を3000にして、1500を中点(Valueに設定)したことを思い出してください。
Trackbarの現在値であるValueは1500を境にして、プラスとマイナス方向になります。
つまり、サーボの角度をdegreeとしたら、degree-1500でプラスマイナスが得られます。

このValueの値とマニュアルにプロトコルを実装してみましょう。
まず、稲妻マークの「デザイン」タブのほうで、ValueChangedイベントを登録します。

20130717blog8
プログラム画面に切り替わりますね。
ここで、次のように入力します。
//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する必要があります。
「デザイン」タブに戻ってください。
20130717blog9

ボタンをセットして、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をダブルクリックして空の関数を出します。

20130717blog10

ここで、もし、serialPort1が開いていたら閉じるプログラムを書きます。
下記を追加してください。

        private void Form1_FormClosed(object sender, FormClosedEventArgs e)
        {
            if (serialPort1.IsOpen == true)
            {
                serialPort1.Close();
            }
        }

これで、フォームが閉じるとき(プログラムの終了時)に自動的にシリアルポートが閉じられることになります。

さて、F5でコンパイルして、動かしてみましょう。

20130717blog11

COMポートとBaudrateを選んで「Set」ボタンを押します。

20130717blog12
シリアルポートがきちんと設定できていれば、このようなウィンドウがでますので、OKを押します。
エラーのときもエラーが出ますので適切なCOMポートとボーレートを選んでください。

20130717blog13
早速、画像ではすでにOFFになっていますが、トルクをONにして、trackbarを動かしてサーボを動かしてみましょう。

すいすい動きますね。
モータのパワーがけっこうありますので、怪我しないように気をつけて動かしてみてください。

次回は、Richtextに返ってくるAckを表示したり、現在位置を取得して表示したりしましょう。
それでは!

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


RSC-U485

RS40X用サーボアーム

バッテリーPR-4S780P

RS405CB

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