シリアルサーボ制御方法（３） ソフト編
はじめに
ソフト編では、ICSコマンドをマイコンからサーボモーターに送るためのC言語のプログラムについて説明します。 ここでは説明を簡単にするため、シリアル通信に関しては１バイトのデータを受信する関数（char getchar()）と１バイトを送信する関数（void putchar ()）ができあがっていることにします。この２つの関数を元にして、シリアルサーボモーターのコマンドの仕組みと通信プログラムの作り方について説明します。 またポジション設定コマンドだけ扱いますので、他のコマンドについてはこれを参考にして作成してみてください。 プログラムは弊社製マイコンボード、KCB-1に付属しているSDK準拠で作成します。KCB-1ではICSコマンドはほとんど関数化されていますので、簡単にシリアルサーボモーターの制御ができます。KCB-1の詳しい情報についてはこちらをご覧ください。
ポジションコマンドの作成
ここではポジション設定コマンドを実行する関数としてset_posというC言語関数を作ってみます。
必要なデータをまとめる
まずICSの規格に沿って、ポジションコマンドを作成しましょう。ポジションコマンドの基本命令は次のようになっています。ポジションコマンドやその他のICSコマンドの詳細は前回の「シリアルサーボ制御方法（２） ICS編」で確認してください。
| CMD | POS_H | POS_L |
CMD＝メインコマンド0b100XXXXXとID＝0b000XXXXXを合わせたものです。この２つのデータを１バイトに合わせるには、論理和（C言語では「｜」と書く）を使います。 POS_Hは3500～11500で指定できるポジションデータの上位７ビットで、POS_Lは下位７ビットデータです。
関数の引数と返値を決定する
set_pos関数に必要なデータは、ID番号とポジションデータです。ID番号は0～31までなので、unsigned char型で指定します。ポジションデータ範囲は3500～11500ですので、int型で指定します。 また、ポジションコマンドを送ると、現在位置データが返ってきますので、この関数の返値は現在位置データとします。 関数の基本構造は次のようになります。ここから中身を記述していきます。
|
1 2 3 4 5 6 |
int set_pos (unsigned char id, int pos) { // これから関数の中身を記述 } |
送信コマンドを作成する
送信コマンドを作成するには３バイトのデータが必要です。バイトデータは0～255までのunsigned char型の３バイト配列「tx」として宣言します。送信するポジションデータの変数名（仮引数名）をposとします。
|
1 2 3 4 5 6 7 8 |
name="code"]int set_pos (unsigned char id, int pos) { unsigned char tx[3]; tx[0] = 0x80 | id; tx[1] = (unsigned char)(pos >> 7 & 0x7F); tx[2] = (unsgined char)(pos & 0x7F); } |
CMDを作成するには、0x80=0b10000000でID番号idと論理和を取ることにより（この場合は単に足し合わせてかまいません）、CMD=0x100XXXXX（XはID番号）ができあがります。実際はid番号が0以上31以下であることを確認してからコマンドを作成した方がよいのですが、ここでは割愛しています。
６行のtx[1]はPOS_Hです。引数のposの下位７ビットを捨てるため、右向きに７ビットシフトしています。またデータを７ビットに限定するために、0x7F（0b01111111）で下位７ビットのみ論理積で取り出しています。 tx[2]はPOS_Lですので、下位７ビットのみ論理積で取り出しています。 変数posはint型変数なので、unsigned char tx[]に型変換（キャスト）してから代入しています。 これで送信コマンドは完成です。
コマンドを送信して、返値を受け取る
上記でできたコマンドを３バイト繰り返して送信します。その後すぐにサーボモーターから返ってくる３バイトのデータを受け取ります。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
int set_pos (unsigned char id, int pos) { unsigned char tx[3]; // 送信用のデータ unsigned char rx[3]; // 受信用のデータ int i; // 繰り返し処理のためにつかう変数 tx[0] = 0x80 | id; tx[1] = (unsigned char)(pos >> 7 & 0x7F); tx[2] = (unsgined char)(pos & 0x7F); for (i = 0; i < 3; i++) { putchar (tx[i]); // コマンドを１バイトずつ送信する } for (i = 0; i < 3; i++) { rx[i] = getchar (); // モーターからの返値を受け取り、rxに代入する } } |
マイコンのクロック数やサーボモーターの種類によっては、送信後にサーボモーターからデータが返ってくるまで少しタイミングをおいた方がよい場合がありますので注意してください。 この例ではマイコンとのデータ通信を想定していますので、受け取るデータは３バイトになりますが、PCで同じような関数を作成した場合は６バイト受け取るようにしてください（詳しくは次回）。
サーボから返ってきたデータから現在値を取り出す
最後にサーボから受け取ったデータから現在位置に変換します。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 |
name="code"]int set_pos (unsigned char id, int pos) { unsigned char tx[3]; // 送信用のデータ unsigned char rx[3]; // 受信用のデータ int i; // 繰り返し処理のためにつかう変数 int dat; // 現在位置を計算するための変数 tx[0] = 0x80 | id; tx[1] = (unsigned char)(pos >> 7 & 0x7F); tx[2] = (unsgined char)(pos & 0x7F); for (i = 0; i < 3; i++) { putchar (tx[i]); // コマンドを１バイトずつ送信する } for (i = 0; i < 3; i++) { rx[i] = getchar (); // モーターからの返値を受け取り、rxに代入する } dat = (int)(rx[1] & 0x7F); dat = (dat << 7) + (int)rx[2]; return dat; } |
16ビットマイコンのCコンパイラでは計算時に32ビット拡張を行わないものがあります。そのような場合８ビットデータを７ビットシフトした時点で８ビット以上の桁は全て０となってしまいますので、上記プログラムのように2段階に分けて、いったんint型変数に代入してからシフト演算をした方がよいでしょう。 計算結果はreturn命令で呼び出し元へ返します。これで関数の完成です。
set_pos関数を使うときには
下のプログラムはID番号が0～8までのシリアルサーボモーターをデイジーチェーン接続して、5000と10000のポジションを10往復する例です。細かい説明はしませんが、参考にしてください。 set_pos関数を繰り返して使う場合は、繰り返し間隔を適当に空ける必要があります。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
int main () { int pos, r_pos; // セットするポジションデータと返ってきたポジションデータ int i, id; // idには適当な数値を入れておくこと for (i = 0; i < 10; i++) // 10回繰り返す { pos = (pos == 5000 ? 10000 : 5000); // ポジションデータを切り替える for (id = 0; id <= 8; id++) { r_pos = set_pos (id, pos); printf ("[%d] pos = %d\n", id, r_pos); } } } |
次回は
次回はいよいよPCからサーボモーターを動かすプログラムをつくります。PCからシリアルサーボモーターを動かすには、近藤科学社製Dual USBアダプターHS、ICS USBアダプターまたはICS USBアダプターHSが必要です。ぜひ準備してお待ちください。
注意
ICS通信は半二重通信ですので、送信・受信のどちらかしか有効にならないようなプログラムを作成する必要があります。マイコンプログラミングでは、送信前に必ず受信禁止状態にしてください。送信後はすぐに受信可能状態に戻すようにしてください。 受信コマンド（getchar）では、仮に何らかのエラーでICSデバイスから返事が来なかったり、またはデータを受け取り損ねたりした場合の処理が必要となります。できれば受信時に適当なカウンター変数を用意して、ある程度カウントしてもデータが来なかった場合には受信エラー処理を行う用に作成すると、受信に失敗した時点でプログラム全体が停止してしまうのを防ぐことができます。
シリアルサーボ制御方法（５） PCから直接制御編（その２）
シリアルサーボ制御方法（４） PCから直接制御編（その１）
シリアルサーボ制御方法（３） ソフト編
シリアルサーボ制御方法（２） ICS編
シリアルサーボ制御方法（１） 回路編
KRS-9303HV ICSの詳細をみる KRS-9304HV ICSの詳細をみる KRS-2552R3HV ICSの詳細をみる KRS-3204R2 ICSの詳細をみる KRS-2572R2HV ICSの詳細をみる KRS-2552R2HV ICSの詳細をみる KRS-2542R2HV ICS の詳細をみる KRS-3204R2 ICSの詳細をみる KRS-2572R2HV ICSの詳細をみる KRS-2552R2HV ICSの詳細をみる KRS-2542R2HV ICS の詳細をみる KRS-9004HV ICSの詳細をみる KRS-5054HV ICS H.Cの詳細をみる KRS-5053HV ICS H.Cの詳細をみる KRS-5034HV ICSの詳細をみる KRS-5033HV ICSの詳細をみる KRS-5032HV ICSの詳細をみる KRS-6104FHV ICSの詳細をみる KRS-3304R2 ICSの詳細をみる KRS-3302 ICSの詳細をみる KRS-6003R2HV ICSの詳細をみる KRS-3301 ICSの詳細をみる KRS-3304 ICSの詳細をみる KRS-3204 ICSの詳細をみる KRS-6003RHV ICSの詳細をみる KRS-2572HV ICSの詳細をみる KRS-4037HV ICSの詳細をみる KRS-4034HV ICSの詳細をみる KRS-4033HV ICSの詳細をみる KRS-4032HV ICSの詳細をみる KRS-4031HV ICSの詳細をみる KRS-2552RHV ICSの詳細をみる KRS-2542HV ICSの詳細をみる KRS-6003HV ICS Red Versionの詳細をみる