VENUS ROBOTIX

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

 

• 1-3行目

シリアル通信のためにいくつかのヘッダファイルをインクルードします。

 

• 5行目

シリアル通信用に用いるファイルディスクリプタ(識別子)をグローバルで定義します。

 

• 7-27行目

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

Linuxでのシリアル通信はネット上に多くの情報がありますので、詳しくはそちらをご覧ください[1]。

 

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

 

void open_port(ポート名, ボードレート)

 

• 9-13行目

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

ポートのオープンには以下の関数を使用します。

 

ファイルディスクリプタ open(ファイル名, パラメータフラグ)

 

ファイル名(ポート名)はコンフィグレーションによって与えることを想定しています。

OpenRTM C++版ではstring型のコンフィギュレーションは、std::string型として与えられます。

openのファイル名はchar *型で与える必要があるので、c_str()メソッドでchar *型の変数に変換します。

パラメータフラグにO_RDWRをセットすることで、読み書きモードでファイルをオープンします。

 

ファイルディスクリプタが負の場合はオープンに失敗しているので、例外をスローします。

 

• 15-16行目

 シリアル通信の設定に使用する構造体を定義し、初期化します。

 

• 18行目

制御モードを設定します。

それぞれの設定値の意味は以下のようになります。

 

baudrate ：ボーレートをbaudrateに指定された値に設定する

CS8 ：文字サイズを8 bitに設定する

CREAD ：受信を有効にする

CLOCAL ：モデムの制御線を無視する

 

• 20-21行目

タイムアウトの設定をおこなします。

各設定項目は以下のようになっています。

 

VTIME ：読み込み時のタイムアウト時間 0.1 s　*この値は環境によっては調整が必要かもしれません

VMIN ：読み込み時の最小文字数

 

• 23行目

デバイスのパラメータを変更します。

パラメータ変更には以下の関数を使います。ioctlは成功した場合は0、失敗した場合は-1を返します。

 

int ioctl(ファイルディスクリプタ, コマンド, ...)

 

コマンドには以下の値を設定します。

 

TCSETS ：現在のシリアルポートの設定を変更します。3番めの引数にtermios構造体を設定します。

 

• 25行目

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

 

• 26行目

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

 

• 29-57行目

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

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

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

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

 

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

 

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

 

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)を計算した値を設定します。

 

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

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

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

 

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

 

• 31行目

コマンドを格納する配列を確保します。

コマンドは1 byteの正数なので、unsigned char型を使用します。

 

• 31-42行目

配列に先ほど説明したコマンドを格納します。

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

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

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

 

• 44-50行目

チェックサムを計算し、配列に格納します。

 

• 52行目

この後、コマンドを出力するので、出力バッファをクリアします。

 

• 54行目

コマンドを出力します。

出力には以下の関数を使用します。

関数が成功すると書き込んだバイト数が返され、失敗した場合は-1が返されます。

 

書き込んだバイト数 write(ファイルディスクリプタ, データのバッファ, 書き込みバイト数)

 

• 56行目

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

*この値は環境によっては調整が必要かもしれません。

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

関数が成功すると0を返し、エラーの場合は-1が返されます。

 

int usleep(中断の時間 us)

 

• 59-81行目

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

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

コマンドは異なりますが、基本的な操作はmove関数と同様です。

 

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

 

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

 

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

 

• 83-115行目

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

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

 

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

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

 

void get_sensor(シリアルサーボのID, センサ情報構造体)

 

• 85-102行目

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

 

• 104行目

この後、センサ値を読み込むので、入力バッファをクリアします。

 

• 105-106行目

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

読み込みには以下の関数を使用します。

関数が成功すると読み込んだバイト数が返され、失敗した場合は-1が返されます。

 

読み込んだバイト数 read(ファイルディスクリプタ, データのバッファ,　読み込みバイト数)

 

• 108-114行目

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

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

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

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

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

 

デコードした各センサ値をRSServo::Sensor構造体に格納します。

 

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

 

• 122行目

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

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

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

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

 

• 130行目

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

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

関数が成功すると0を返し、失敗した場合は-1が返されます。

 

int close(ファイルディスクリプタ)

 

• 138-146行目

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

 

• 148-160行目

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

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

 

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

コードが入力し終わったら、makeを行ってください。

 

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

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

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