VENUS ROBOTIX

　それではソースコードを解説します。

 

• 1行目

シリアル通信のためにPYSERIALをインポートします。

 

• 16行目

シリアルオブジェクトをインスタンス化します。

 

• 20-30行目

シリアルポートをオープンする関数です。

open_port関数は以下のようになります。

 

open_port(ポート名, ボードレート, タイムアウト)

 

• 21行目

シリアルポートを指定します。

 

• 22行目

ボーレートを指定します。

 

• 23行目

タイムアウトを設定します。

タイムアウトの値は以下のようになっています。

None： 読み取れるまで待ちます

0： 非ブロッキングモード（読み取り時にすぐ戻ります）

x： x s待ちます

 

• 25-30行目

シリアルポートを開きます。

成功した場合は開いたポート番号を、失敗した場合はエラーメッセージを表示します。

 

• 32-33行目

シリアルポートをクローズじます。

close_port関数は以下のようになります。

 

close_port()

 

• 35-40行目

シリアルサーボに保持トルクのON/BRAKE/OFF指令を与える関数です。

保持/非保持/ブレーキモードから選択して動作させます。

 

on関数は以下のようになります。

 

void on(シリアルサーボのID, 保持モードのモード)

 

各コマンドやデータの意味は簡単に説明します。

詳しくは、RS405CB/RS406CB取り扱い説明書[1]をご覧ください。

RSシリーズのシリアルサーボには以下のような形式でコマンドを送信します。

 

[Header] [ID] [Flags] [Address] [Length] [Count] [Data] [Sum]

 

Header ：パケットの先頭を表します。ここでは0xFA、0xAFとなります。

ID ：シリアルサーボのIDです。

Flags ： リターンパケットの設定やデータ書き込み時の設定を行います。

Address ：メモリーマップ上のアドレスを表します。Lengthに指定した長さのデータをメモリーマップに書き込みます。

Length ：データの長さ(byte数)を指定します。

Count ：サーボの数を表します。今回は1に設定します。

Sum ：チェックサムです。IDからDataまでの各バイトの排他的論理和(XOR)を計算した値を設定します。

 

on関数では、以下のようなバイト列で動作指令を与えます。

 

|0xFA|0xAF|ID|0x01|0x24|0x01|0x01|保持トルクのモード|Sum|

 

 

保持トルクのモードは、0x00で非保持、0x01で保持, 0x02でブレーキとなります。

 

• 37行目

リストに先ほど説明したコマンドを格納します。

 

• 38行目

チェックサムを計算し、リストに追加します。

 

• 40行目

コマンドを出力します。この関数は後ほど説明します。

 

 

• 42-50行目

シリアルサーボに動作指令を与える関数です。

シリアルサーボの角度(位置)と移動時間を与えて、動作させます。

 

move関数は以下のようになります。

 

move(シリアルサーボのID, 角度, 移動時間)

 

move関数では、以下のようなバイト列で動作指令を与えます。

目標位置と時間は1 byteを超えるため、上位(H)と下位(L)に分けて設定します。

各| |は1 byte単位となります。

 

|0xFA|0xAF|ID|0x01|0x1E|0x04|0x01|目標位置L|目標位置H|時間L|時間H|Sum|

 

• 44-48行目

リストに先ほど説明したコマンドを格納し、出力します。

位置と時間を上位バイトと下位バイトに分けるには、AND演算とビットシフトを用います。

下位バイトは0x00FFと論理的ANDを取ることで、上位ビットは全て0となり、下位ビットは元の値を残せます。

上位バイトは0xFF00と論理的ANDを取り、上位ビットを残した後に1 byteのサイズに収まるよう8 bitシフトしています。

 

• 50行目

シリアルサーボが動作している間スリープします。

スリープには以下の関数を使用します。

 

time.sleep(中断の時間 s)

 

• 53-75行目

シリアルサーボのセンサ値を取得する関数です。

これまでの関数と異なり、シリアルサーボからの返信を受け取ります。

 

get_sensor関数は以下のようになります。

センサ情報構造体とは、IDLで定義したRSServo::Sensorのことです。

 

get_sensor(シリアルサーボのID)

 

• 55-58行目

コマンドの出力まではこれまでと同様です。

 

• 60行目

センサ値を読み込みます。

読み込みには以下のメソッドを使用します。

 

read(読み込みバイト数)

 

• 60-70行目

先ほどまで2 byte以上のデータをエンコードしてコマンドを作成していました。

今度はこれと逆のことをして、センサ値をデコードします。

id以外は2 byteのデータなので、2 byteのデータを復元します。

上位バイトと下位バイトを合わせるには、ビットシフトとOR演算を用います。

上位ビットを8 bitずらし、下位ビットと論理的ORを取ります。

なお、読み込んだセンサ値は8bit文字列になっているので、ord関数を使ってバイトの値に変換しています。

 

もし、値が読み込めていなければ例外をスローします。

 

• 72-73行目

このサーボモータは-1500から1500の角度をとります。

1500よりも大きい値は二の歩数で表現された負の値でので、変換しています。

実際には1500より大きい値が返されることがあるので、マージンをとって1800より大きい値を変換しています。

 

• 77-81行目

この関数は他の関数の処理を助けるヘルパー関数です。

 

_calc_checksum(チェックサムを除いたコマンド)

 

チェックサムの計算を行い、値を返します。

 

• 83-86行目

この関数は他の関数の処理を助けるヘルパー関数です。

 

_write_serial(コマンド)

 

コマンドの出力を行います。

 

• 84行目

出力バッファをクリアします。

 

• 85行目

入力バッファをクリアします。

 

• 86行目

コマンドを出力します。

 

ここからは、上記の関数を使用してRTC本体の挙動をプログラムします。

 

• 89行目

ポートをオープンします。

引数のポート名とボーレートにはコンフィグレーションを与えます。

このようにすることで、ユーザーの環境に合わせてRTCを外部から操作できます。

このように環境により異なるものにはコンフィグレーションを使用します。

 

• 94行目

ポートをクローズします。

ポートのクローズには以下の関数を使用します。

 

• 99-104行目

サーボ指令がある場合は、シリアルサーボに保持トルクのON/BRAKE/OFF指令を与えます。

 

• 108-128行目

動作指令がある場合は、シリアルサーボに動作指令を与えます。

その後、センサ値を取得しセンサ値ポートから出力します。

 

　以上で、RSServoManagerのコーディングは終了です。

 

今回はコードが長く、大変でしょうが頑張って最後までコーディングしてみて下さい。

RSシリーズ以外のシリアルサーボでも、同様の構造でRTC化できると思います。

ぜひ、お手元のシリアルサーボでお試し下さい。