【C#連載】とりあえず通信にトライ!2

お知らせ

先週はアクセスが少なくて、がっかりしてガチ系のブログを書くのやめようかと悩んだyukiです。
FBで友達に聞いたら、ガチ系の技術ブログはロングテイルだからと励ましてもらって気を取り直したところです。

ところで、皆さん、ネコは好きですか?
私はネコと遊んでみたいと思うときはネコ喫茶に行くのですが、世の中なかなか相性のいいネコというものがいません。秋葉のネコ喫茶でも、いっぱいネコいるのに、1匹しか私と遊んでくれません…。ネコハーレムを作っている常連さんがうらやましい限り。

なので、たまたまネコとの相性がよくて、触らせてくれたり、遊んでくれたりすると、プロトコルが合ったとか勝手に思って喜んだりします。ネコと戯れるのはいろいろ発見があってよいですね。
Hi everybody, I’m yuki.
I disappointed less access for my technical blog in last week.
My friends of FB encouraged me to continue technical blog.

By the way, do you love cat?
I go to “Cat Cafe” when I want to play with cat in Akihabara.
However, cat is cat. A few cats play with me even if many cats are in the cafe.
I envy frequenter who makes cat harem.

ここでプロトコルというのは、通信手順のことをいいます。
つまり、ネコとの対話(?)において、ネコが気に入る手順でアプローチできたということです。
ネコが気に入ってくれたので、遊んでくれる。
ここは、インタラクティブな知能、相手がこう思っているだろう、ここではネコが気に入ってくれるだろうという観察に基づく仮説と実行するための行動計画がなされているわけですね。
さらにネコが遊んでくれれば、うまくいったということでフィードバックされて学習もします。
人間ってなんてすごいんだろうって思う瞬間です。w

Σ(@o@)
これだけだとただのプログラムオタのネコ好き話で終わってしまう。
もうちょっと高尚なことを話さないと!!

ロボットを作るときは知的な動作をさせたいと思うものです。
知能は、観察者によって発現する、というのは人工知能研究者にはよく知られた言葉です。

私ごときが言っても信じてもらえないとおもうので、エピソードを紹介すると。w
掃除ロボットroombaで有名なブルックス博士は、当時新米大学助手だった私に、サブサンプションアーキテクチャは奥さんの実家で何もすることがなかったんで、蟻の群れを見てて思いついた。と飲みに行ったカラオケ屋で話してました。
「ネコと戯れる」、こんなところにも、よくよく観察すると人工知能問題が隠れていたりして、面白いです。
皆さんの周りにも潜んでいると思うので探してみてください。w

さて、皆さん、先週のプログラムは動きましたか?
今日は、先週のプログラムの解説としてDynamixelを動かすためのプロトコルに切り込んでいきます。

Dynamixelの場合、プロトコルと言っても単体で動かす場合は、パケットと呼ばれる通信する一連のデータを作って、シリアルで送り込むだけなので、割とシンプルです。

では、まず、パラメータの構造から見て行きましょう。

ROBOTIS MX-64Rのマニュアルを見てください。ここから抜粋した下記がMX-64Rのパラメータ用のマッピングです。

Area

アドレス(16進数)

名称

意味

アクセス

初期値(16進数)

E

E

P

R

O

M

0 (0X00)

Model Number(L)

モデル番号の下位バイト

R

54 (0X36)

1 (0X01)

Model Number(H)

モデル番号の上位バイトr

R

1 (0X01)

2 (0X02)

Version of Firmware

ファームウェアバージョンの情報

R

3 (0X03)

ID

ダイナミクセルのID

RW

1 (0X01)

4 (0X04)

Baud Rate

ダイナミックセルの通信速度

RW

34 (0X22)

5 (0X05)

Return Delay Time

応答遅延時間

RW

250 (0XFA)

6 (0X06)

CW Angle Limit(L)

時計回りの限界角値の下位バイト

RW

0 (0X00)

7 (0X07)

CW Angle Limit(H)

時計回りの限界角度値の上位バイト

RW

0 (0X00)

8 (0X08)

CCW Angle Limit(L)

反時計回りの限界角度値の下位バイト

RW

255 (0XFF)

9 (0X09)

CCW Angle Limit(H)

反時計回りの限界角度値の上位バイト

RW

15 (0X0F)

11 (0X0B)

the Highest Limit Temperature

内部限界温度

 RW

80 (0X50)

12 (0X0C)

the Lowest Limit Voltage

最低限界値電圧

RW

60 (0X3C)

13 (0X0D)

the Highest Limit Voltage

最高限界値電圧

RW

160 (0XA0)

14 (0X0E)

Max Torque(L)

トルク限界値の下位バイト

RW

255 (0XFF)

15 (0X0F)

Max Torque(H)

トルク限界値の上位バイト

RW

3 (0X03)

16 (0X10)

Status Return Level

応答レベル

RW

2 (0X02)

17 (0X11)

Alarm LED

アラーム用LEDの機能

RW

36 (0X24)

18 (0X12)

Alarm Shutdown

アラーム用シャットダウン(Shut down)機能

RW

36 (0X24)

R

A

M

24 (0X18)

Torque Enable

トルクのOn / Off

RW

0 (0X00)

25 (0X19)

LED

LED On/Off

RW

0 (0X00)

26 (0X1A)

D Gain

Derivative Gain

RW

0 (0X00)

27 (0X1B)

I Gain

Integral Gain

RW

0 (0X00)

28 (0X1C)

P Gain

Proportional Gain

RW

32 (0X20)

30 (0X1E)

Goal Position(L)

目標位置値の下位バイト

RW

31 (0X1F)

Goal Position(H)

目標位置値の上位バイト

RW

32 (0X20)

Moving Speed(L)

目標速度値の下位バイト

RW

33 (0X21)

Moving Speed(H)

目標速度値の上位バイト

RW

34 (0X22)

Torque Limit(L)

トルク限界値の下位バイト

RW

ADD14

35 (0X23)

Torque Limit(H)

トルク限界値の上位バイト

RW

ADD15

36 (0X24)

Present Position(L)

現在位置値の下位バイト

R

37 (0X25)

Present Position(H)

現在位置値の上位バイト

R

38 (0X26)

Present Speed(L)

現在速度値の下位バイト

R

39 (0X27)

Present Speed(H)

現在速度値の上位バイト

R

40 (0X28)

Present Load(L)

現在荷重値の下位バイト

R

41 (0X29)

Present Load(H)

現在荷重値の上位バイト

R

42 (0X2A)

Present Voltage

現在の電圧

R

43 (0X2B)

Present Temperature

現在の温度

R

44 (0X2C)

Registered

Instructionの登録状況

R

0 (0X00)

46 (0X2E)

Moving

移動の有無

R

0 (0X00)

47 (0X2F)

Lock

EEPROMのロック

RW

0 (0X00)

48 (0X30)

Punch(L)

Punch値の下位バイト

RW

0 (0X00)

49 (0X31)

Punch(H)

Punch値の上位バイト

RW

0 (0X00)

68 (0X44)

Current(L)

モータが消費している電流量の下位バイト

R

69 (0X45)

Current(H)

モータが消費している電流量の上位バイト

R

70 (0X46)

TorqueControl Mode Enable

トルク制御モード

RW

0 (0X00)

71 (0X47)

GoalTorque(L)

目標トルク値の下位バイト

RW

0 (0X00)

72 (0X48)

GoalTorque(H)

目標トルク値の上位バイト

RW

0 (0X00)

73 (0X49)

GoalAcceleration

目標加速度値

RW

0 (0X00)

これは、メモリーマップと言って、サーボのボードCPUのメモリに書いてある内容です。
この番地に書き込んだデータが、サーボの制御に使用されるため、ここを書き間違うとサーボが壊れて工場に修理送りになりますので要注意です。(ファームを壊さなければほとんどの場合は手元で直せますが、うちはいろいろ試しすぎてファームからぶっとばしていくつか壊しました。w)

サーボは、RS485でパケットと呼ばれる一連のコマンド列を読み取ります。
このコマンド列であるパケットは次のようになっています。

パケットを図示するとのようになります。

では、パケットの中身を細かく見て行きましょう。
実際のプログラムでのパケット構成を見るとわかりやすい人もいると思うので、button1(LED ON)を押したときの動作の関数となるプログラムをあわせて見てください。

        private void button1_Click(object sender, EventArgs e)
        {
            byte[] param = new byte[8];	//byte型で初期化、全体の長さは8
            int size = 0;//パケットサイズを計算するために初期化
            int id = 1;//IDの設定

            //パラメータのセット
            param[size++] = 0xFF;                   //ヘッダー
            param[size++] = 0xFF;                   //ヘッダー
            param[size++] = (byte)(int)(id & 0xFF); //ID
            param[size++] = (byte)4;                //パラメータのバイト数+2
            param[size++] = (byte)3;                //2: READ, 3: WRITE
            param[size++] = 0x19;                   //LED map address
            param[size++] = 1;                      // 1: on 0: off
            param[size++] = calc_checksum_robotis(param);
 
            //サーボの書き込み
            if (serialPort1.IsOpen == true)
            {
                serialPort1.Write(param, 0, size);
            }
        }

最初の2バイトは、0xFF, 0xFFで始まります。
これは、パケットの始まりであることを示します。
パケットでは、パケットの始まりを示すバイト(列) のことをヘッダーといいます。
ここ、とても重要ですので、またこの先の連載でも解説します。

次は、サーボのIDです。
サーボはマップの4番目を見ると1バイトデータです。
原理的には255までIDが設定できることになります。しかし、Dynamixelでは、254がブロードキャストIDと言って、一斉通信をするためのIDになりますから、1~253までが使えるIDになります。初期設定値は1です。手持ちのもので、サーボを買ったばかりであれば、IDは1になっているはずです。もうすでにID設定を変えているのであれば、そのIDを指定します。
プログラムの5行目と10行目を見てください。5行目のプログラム中のマーカーになっている部分がIDの番号設定です。
下のプログラムで書いてある10行目の(id&0xFF)は、byte型にキャスト(型あわせの意味)するために確実に8ビット分のデータを確保するためのものです。(自分が絶対に間違えない自信があるなら、下記のようにparam[size++]=(byte)idと書いてもよい)

            param[size++] = (byte)id ; //ID

次は、11行目、Length(長さ)を指定します。
これは、パラメータのバイト数+2(チェックサムと指令の分らしい)の数値を設定します。
ここもパケットデータの指定によると1バイトのため、1バイト255 – 2 = 253までの長さのパラメータが設定できることになります。
したがって、パラメータはどんなに長くても、253個までです。
今回の設定では、LEDを設定するので、マップをみるとLEDのパラメータは1バイトです。
ON/OFFの1バイト指令しかありませんから書き込むマップの場所は1バイト、パラメータは1バイトの合計2バイト+規定の2バイト=4となります。

次のバイト12行目は、コマンド指令です。
このパケットがサーボに何をさせたいのかを指令します。
コマンドはいろいろあるのですが、今回はREAD/WRITEに限ります。
READのコマンドは2、WRITEのコマンドは3です。
READは、サーボに状態を聞いて返信させるコマンドです。
WRITEは、サーボに指令を与えるコマンドです。
今回はLEDのON/OFF指令を与えますのでWRITE=3を使います。

その次13行目がLEDのマップアドレスです。
このマップの番地にパラメータを書き込むことによって、サーボはそのデータを読み出して動作します。マップで見るとLEDの番地は、0x19(16進数)です。

その次14行目は、LEDのマップ番地に書き込む変数です。マップの解説によると1バイトです。
ここでは、LEDをON=1にしたいので、1を書き込みます。

ここまででパラメータができました。

このようなマップを使ってサーボへの指令を送るパケットを作ります。

さて、ここで15行目に、数値ではなくチェックサムを計算して返す関数が最後に来ています。
チェックサム (Check Sum)とは誤り検出符号の一種です。
チェックサムとして符号値そのものを指すこともありますが、Dynamixelでは計算する必要があります。
前の週に書き足してもらった次のプログラムを見て下さい。

        //ROBOTIS用のチェックサムの計算
        private byte calc_checksum_robotis(byte[] packet)
        {
            int checksum = 0;
 
            for (int i = 2; i < packet.Length - 1; i++)
            {
                checksum += (int)packet[i];
            }
 
            checksum = (~checksum) & 0xFF;
 
            return (byte)(checksum&0xFF);
        }

ここで calc_checksum_robotis()の関数は、private byteという型になっています。つまり、データをはめ込むparamがbyteなので、byte型のデータを返す必要があり、この関数はprivate byte型になっています。

Dynamixelのチェックサムは、IDから最後のチェックサムの一つ手前までのバイトをすべて足し算した数値の1バイト分の補数の数値になります。IDは先ほど説明したパケットでは3番目(C#では、0から数えはじめるので3番目は2です。つまりi=2)から最後の一つ手前packet.Length-1までです。そのようにfor文でループさせて計算します。

ところで、1バイト=255までの数字ですから、byte型は最大255までで計算をする必要があります。
しかし、計算するとどう考えても足し算していったら255を越えてしまいます。

そこで、数値としては4バイトまで数えられるint型で計算して、最後にbyte型に変換するという方法をとります。
計算式で見ると、checksumはint型(4バイト)で計算してますね。
11行目で補数を計算(~の記号が補数計算)する (~checksum) & 0xFFになっています。&0xFFで1バイト分を切り出しています。
最後の最後でbyte型に変換しています。

チェックサムを計算してパケットにつけて送ることで、パケットの内容が合っているかどうかをサーボ側、PC側でも確認ができます。
#ちなみになぜかDynamixelはパケットのチェックサムが間違っててもサーボはコマンドを実行しますので注意。一応チェックサムが間違ってるとパケットが間違ってるよという返事もきます。

今回作ったパケットは、このような内容でした。
このように、byte型を使って、パラメータとパケットの作り方がわかれば、あとはシリアルポートから送るだけです。
この手順(プロトコル)は17~21行目に書いてあるように思いきり簡単です。

  1. シリアルポートが開いているか確認する。
  2. シリアルポートがあいていたら、パケットparamをセットし、パケットの何番目(今回は0から)から、何番目(今回はsize分)までを送ると書く
    serialPort1.Write(param, 0, size);

皆さん、シリアルポートの設定とByteの扱いはなんとなくつかんでいただけたでしょうか?
これからいろいろ便利な(複雑な?)プログラムを追加していきます。

次回は、デバイスマネージャーをいちいち見に行かなくてもシリアルポートを読み込む設定について解説します。
お楽しみに!

ロボコンマガジン、ぜひどうぞ。マイクロものづくりは最近話題の本。

【C#の入門書とか】

I’m happy to match protocol with cat when cat plays with me or touching if cat likes me.
It is good to find something when I play with cat.

“protocol” is protocol to communicate with someone else.
In the dialog with cat, protocol succeeded in cat means I could approach to cat according to cat protocol.

When cat played with me, cat likes me.
In this step, interactive intelligence and thinking simulation, here cat likes, behavior planning has been simulated in my mind based on observation and hypothesis of cat protocol.
Add to say, we can learn something from feedback if cat plays.
It is the the moment of thinking “man is great”.

Σ(@o@)
Hmm, it is just cat lover stories.
I’ll say some intelligent things.

We hope to make intelligent behavior to robot.
It is the famous sentence for AI researcher, “Intelligence emerges by observer.”

I tell you episode of Dr.Brooks.
When I was research assistant, I met him in conference of AI and went to drink.
Dr.Brooks who is famous of roomba, he told me that he inspired ant group behavior to subsumption architecture at his wife’s home.

I beleive you can find something of AI in playing with cat.

Ok, let’s see program from last proram.
Have you turned on LED on servo?
I show you protocol to drive Dynamixel with program.

In the case of Dynamixel, it is simple to make packet and transmit from serial port.
Let’s see structure of parameter and packet.

Please refer ROBOTIS MX-64R manual. They are the parameter mapping of MX-64R.

Area

アドレス(16進数)

名称

意味

アクセス

初期値(16進数)

E

E

P

R

O

M

0 (0X00)

Model Number(L)

モデル番号の下位バイト

R

54 (0X36)

1 (0X01)

Model Number(H)

モデル番号の上位バイトr

R

1 (0X01)

2 (0X02)

Version of Firmware

ファームウェアバージョンの情報

R

3 (0X03)

ID

ダイナミクセルのID

RW

1 (0X01)

4 (0X04)

Baud Rate

ダイナミックセルの通信速度

RW

34 (0X22)

5 (0X05)

Return Delay Time

応答遅延時間

RW

250 (0XFA)

6 (0X06)

CW Angle Limit(L)

時計回りの限界角値の下位バイト

RW

0 (0X00)

7 (0X07)

CW Angle Limit(H)

時計回りの限界角度値の上位バイト

RW

0 (0X00)

8 (0X08)

CCW Angle Limit(L)

反時計回りの限界角度値の下位バイト

RW

255 (0XFF)

9 (0X09)

CCW Angle Limit(H)

反時計回りの限界角度値の上位バイト

RW

15 (0X0F)

11 (0X0B)

the Highest Limit Temperature

内部限界温度

 RW

80 (0X50)

12 (0X0C)

the Lowest Limit Voltage

最低限界値電圧

RW

60 (0X3C)

13 (0X0D)

the Highest Limit Voltage

最高限界値電圧

RW

160 (0XA0)

14 (0X0E)

Max Torque(L)

トルク限界値の下位バイト

RW

255 (0XFF)

15 (0X0F)

Max Torque(H)

トルク限界値の上位バイト

RW

3 (0X03)

16 (0X10)

Status Return Level

応答レベル

RW

2 (0X02)

17 (0X11)

Alarm LED

アラーム用LEDの機能

RW

36 (0X24)

18 (0X12)

Alarm Shutdown

アラーム用シャットダウン(Shut down)機能

RW

36 (0X24)

R

A

M

24 (0X18)

Torque Enable

トルクのOn / Off

RW

0 (0X00)

25 (0X19)

LED

LED On/Off

RW

0 (0X00)

26 (0X1A)

D Gain

Derivative Gain

RW

0 (0X00)

27 (0X1B)

I Gain

Integral Gain

RW

0 (0X00)

28 (0X1C)

P Gain

Proportional Gain

RW

32 (0X20)

30 (0X1E)

Goal Position(L)

目標位置値の下位バイト

RW

31 (0X1F)

Goal Position(H)

目標位置値の上位バイト

RW

32 (0X20)

Moving Speed(L)

目標速度値の下位バイト

RW

33 (0X21)

Moving Speed(H)

目標速度値の上位バイト

RW

34 (0X22)

Torque Limit(L)

トルク限界値の下位バイト

RW

ADD14

35 (0X23)

Torque Limit(H)

トルク限界値の上位バイト

RW

ADD15

36 (0X24)

Present Position(L)

現在位置値の下位バイト

R

37 (0X25)

Present Position(H)

現在位置値の上位バイト

R

38 (0X26)

Present Speed(L)

現在速度値の下位バイト

R

39 (0X27)

Present Speed(H)

現在速度値の上位バイト

R

40 (0X28)

Present Load(L)

現在荷重値の下位バイト

R

41 (0X29)

Present Load(H)

現在荷重値の上位バイト

R

42 (0X2A)

Present Voltage

現在の電圧

R

43 (0X2B)

Present Temperature

現在の温度

R

44 (0X2C)

Registered

Instructionの登録状況

R

0 (0X00)

46 (0X2E)

Moving

移動の有無

R

0 (0X00)

47 (0X2F)

Lock

EEPROMのロック

RW

0 (0X00)

48 (0X30)

Punch(L)

Punch値の下位バイト

RW

0 (0X00)

49 (0X31)

Punch(H)

Punch値の上位バイト

RW

0 (0X00)

68 (0X44)

Current(L)

モータが消費している電流量の下位バイト

R

69 (0X45)

Current(H)

モータが消費している電流量の上位バイト

R

70 (0X46)

TorqueControl Mode Enable

トルク制御モード

RW

0 (0X00)

71 (0X47)

GoalTorque(L)

目標トルク値の下位バイト

RW

0 (0X00)

72 (0X48)

GoalTorque(H)

目標トルク値の上位バイト

RW

0 (0X00)

73 (0X49)

GoalAcceleration

目標加速度値

RW

0 (0X00)

They are memory map which shows memory in CPU of servo motor.
Data must be written in this address to control servo. It is important to write correctly not to harm servo motor. We had to ship broken servo motors to factory to repair.

Servo motor reads command from packet through RS485 communication.
The packet is as follows.

This is the figure of packet.

Let’s see details of packet.
Please see program button1 (LED ON) also if it is easy for you to learn with program.

        private void button1_Click(object sender, EventArgs e)
        {
            byte[] param = new byte[8];	//Initialize byte length 8
            int size = 0;//initialize packet size
            int id = 1;//ID setting 

            //set parameter
            param[size++] = 0xFF;                   //header
            param[size++] = 0xFF;                   //header
            param[size++] = (byte)(int)(id & 0xFF); //ID
            param[size++] = (byte)4;                //byte numbers+2
            param[size++] = (byte)3;                //2: READ, 3: WRITE
            param[size++] = 0x19;                   //LED map address
            param[size++] = 1;                      // 1: on 0: off
            param[size++] = calc_checksum_robotis(param);
 
            //write servo
            if (serialPort1.IsOpen == true)
            {
                serialPort1.Write(param, 0, size);
            }
        }

First 2bytes are starting from 0xFF 0xFF.
They show start of packet.
We call “header” if byte stream shows packet.
I’ll introduce header role later.

Next sentence is servo ID.
Servo ID in map is 4th and 1 byte data.
According to principal, ID can be set 255. But Dynamixel has rule what 254 is broadcast ID to communicate all servo motors with this number. Therefore, we can use 1-253 for servo motor ID.
Default ID number is 1. ID must be 1 if you didn’t change ID yourself. Please change here with your ID if you set another ID number.
Please see 5th and 10th line in the program. In 5th line, id is the number setting line.
Following 10th line changes type from int to byte (cast). You can write as follows if you have absolute confidence to get byte size data.

            param[size++] = (byte)id ; //ID

In 11th line, we put length data of parameter.
We put byte numbers of parameter + 2 (for size of checksum and command).
According to packet, here size is 1 byte. The length size 1 byte = 255-2 = 253 are permitted to set. Max 253 can be used for this packet.
In this setting, we use LED. LED map suggests 1 byte for data.
Therefore, map address is 1 byte and data must be ON/OFF 1 byte. Parameter 2 byte +2 = 4 byte to put here.

12th line is command.
This byte put command what to do for servo motor.
We have several commands for servo motor, but we handle read/write only.
READ command is 2, WRITE command is 3.
READ means to get reply from servo.
WRITE means to put command to servo what to do.
In this program, we use WRITE=3 to turn on/off LED.

13th line is map address of LED.
Servo motor refers this address to decide behavior. According to map, LED address is 0x19 in
hexadecimal number.

14th line is LED map paramter 1byte.
We want to turn on LED ON=1.

We’ve done to make parameters in packet.

We make packet with map address and commands.Please refer to detail of pakcet
here

Look at 15th line.
15th line shows function which calculate checksum.
A checksum is a small-size datum computed from an arbitrary block of digital data for the purpose of detecting errors that may have been introduced during its transmission.
Sometime checksum is just number, but Dynamixels requires to calculate parameters.

Please see program which I asked to add.

        //Checksum calculation for ROBOTIS
        private byte calc_checksum_robotis(byte[] packet)
        {
            int checksum = 0;
 
            for (int i = 2; i < packet.Length - 1; i++)
            {
                checksum += (int)packet[i];
            }
 
            checksum = (~checksum) & 0xFF;
 
            return (byte)(checksum&0xFF);
        }

Function calc_checksum_robotis() is the type of “private byte.” Because param in program which calls this function is “byte”.

Checksum for Dynamixel sums up all data from ID to parameter of before checksum and complement of sum. From order of ID is 3rd but i=2 because C# counts from 0, to pakcet.Length -1 to put for loop.

By the way, 1byte = 8bit = 256, byte type can count from 0 to 255. 1 byte overs 256 if you sum up all parameters. We use int (4byte) to sum up. Then we get complement and cast the byte type at last.

Please see program.
Checksum is calculated by int(4byte).
In line 11, complement (~) and 0xFF are used to get byte size.
At last, checksum cast by (byte)

Servo motor and PC can confirm packet parameters correct or not with checksum.
#I found Dynamixel moves if we send wrong checksum. I was surprized but servo replied this packet is wrong checksum.

We’ve learned how to make packet for Dynamixel.
We learned byte type to make parameter and pakcet to transmit from serial port.
It is easy protocol to refer to 17 -21 line.

  1. confirm serial port to be opened.
  2. Packet “param” set, first packet byte number = 0 set, last packet byte number = size set as follows.
    serialPort1.Write(param, 0, size);

Have you learned serial port and byte cast?
We’ll try to add convinient (complex?) prgram.

I’ll show you how to get serial port number not to see device manager.
See you next week!

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