１５３．シーケンシャルな連続動作をさせる

2001/07/08 【ソフトウエア編ＴＯＰに戻る】

シーケンシャルな連続動作とはどういうことかと言うと、あらかじめ決められたパターンに従って、連続的に各関節を動かしていくということです。これにより、複数のパターンを用意しておけば、ロボットはその通りに動いてくれるようになります。

今回作成したプログラムでは、CSV形式のテキストファイルでパターンを作成しておき、それをシリアル通信で読み込んでから、そのパターンの動作をロボットに再現させます。今回もパソコンとのシリアル通信にはハイパーターミナルを使用します。

ただし、今回のソフトはモーター８個までの制御に対応です。全部で１２個ある足腰のモーター全てには対応していません。

１５３−１．ロボット操作のためのコマンド

今回のプログラムでは、ハイパーターミナル上から以下のコマンドを送信することによって、ロボットを操作します。

各コマンドは、半角大文字に限ります。上記以外の文字が入力された場合は、無視されます。

１５３−２．データテーブルの書式

今回のロボットの動きは、全てこのデータテーブルに従って行なわれます。データテーブルは、以下の書式に従って作ります。文字は全て半角大文字です。各パラメータの間は全てカンマで区切り、停止位置の後はリターン（改行）しておきます。

コマンド（半角大文字），関節（２桁数字），速度（１桁数字），停止位置（２桁数字）

作成するデータテーブルは、以上の行が複数連続して記述される形になります。

■コマンド

シーケンシャル動作の仕方を指定します。半角大文字です。

■関節

動かしたい関節を指定します。必ず２桁の数字で指定します。

ただし、コマンドがW（待機時間指定）の場合のみ、ここでは待機時間を指定します。待機時間は0.1秒単位を２桁の数字で指定します。30と入力すれば3.0秒、15と入力すれば1.5秒となります。

■速度

関節を動かす速度を指定します。必ず１桁の数字で指定します。

■停止位置

関節を停止させる位置を指定します。必ず２桁の数字で指定します。

各関節の可動範囲については、アクチュエータ編９．関節角度とポテンショメータの調整を参照して下さい。

以上の各パラメータで、データテーブルを作成します。データテーブルはテキストエディタで作ります。以下はデータテーブルの一例です。なお、ファイルの中身はCSV形式ですが、拡張子はTXTにしておきます。その方がハイパーターミナルからの送信が簡単なのです。

一番最初の行（テーブル）は、必ずEコマンドからはじめます。そして、各行（テーブル）の終わりは改行で締めくくります。上のデータテーブルは、以下の動作を行ないます。

E,03,1,22　→　左足首を１の速度で２２の位置まで移動させる。

E,07,1,22　→　ひとつ前のテーブルと同時に、右足首を１の速度で２２位置まで移動させる。

W,30,0,00 →　ひとつ前までの動作が終了してから、３秒間待つ。

S,03,1,16　→　ひとつ前までの動作が終了してから、左足首を1の速度で１６の位置まで移動させる。

E,07,1,16　→　ひとつ前のテーブルと同時に、右足首を1の速度で１６の位置まで移動させる。

Q,00,0,00　→　これでシーケンシャル動作を終了する。

このテーブルに従って動作させると、両足首を少し曲げ、３秒経ったら両足首を少し伸ばす、という動作になります。

【先頭に戻る】

１５３−３．プログラム全体の流れ

このプログラム rbtest2.c は、大体以下の流れで動作します。なお、今回のプログラムは、このページ上に掲載しません。（だらだらと長すぎるため）　プログラムソースrbtest2.cは、ここをクリックするとダウンロードできますので、参考にして下さい。

なお、スタートアップオブジェクトは、今回割込みを利用しているので、startup5.marでないと動作しません。startup5.marはstartup.marに名前を変更してから、ソフトウエア編の３．コンパイル手順 ３−１．スタートアップオブジェクトを作るを参照して、startup.objファイルを作成し、rbtest2.objとリンクして下さい。

■各部の説明にジャンプします。 【１．準備】 【２．メイン関数】main() 【３．データ読み込み関数】csv_conv() 【４．データテーブル一覧表示関数】table_list() 【５．関節位置一覧表示関数】pos_list() 【６．シーケンシャル動作関数】rb_move() 【７．PWM波形発生関数】intimia2()

【先頭に戻る】

１５３−４．プログラム rbtest2.c の動作説明

１５３−４−１．【１．準備】の説明

■ヘッダファイルの読み込み

今回のプログラムでは、おなじみのヘッダフィル3048f.hの他に、今まで自分で作ってきた汎用的な関数を収めたmyfunc.hも利用します。myfunc..hについては２０１．自分の関数をヘッダファイルにするを参照して下さい。

■割込み関数の宣言

今回、PWM波形を発生させるため、ITU2のタイマー割込みを利用しています。なので、#pragma interrupt(intimia2)として、intimia2()の関数を割込み関数として宣言しています。

同様に、データテーブルでW（待機）コマンドが指定された場合のタイマー動作をさせるため、ITU1のタイマー割り込みも利用するため、#pragma interrupt(intimia1)として、intimia1()の関数も割込み関数として宣言しています。

■シーケンスデータテーブル用の変数宣言

シーケンス用のデータテーブルのためのメモリ領域として、以下の各領域を確保しています。

extern char G_COMM; /* コマンド種別テーブル 100Byte分 */
extern int G_MOTOR;  /* 関節指定テーブル 200Byte分 */
extern int G_SPEED;   /* 速度指定テーブル 200Byte分 */
extern int G_POS;      /* 停止位置テーブル 200Byte分 */

ここで確保している領域のサイズは、startup5.marの中のres. コマンドで指定されています。読み込んだCSV形式のテキストファイルは、上記の各種パラメータごとに整理されて、それぞれのメモリ領域に順番に格納されて行きます。char型は１パラメータ当たり１Byte、int型は１パラメータ当たり２Byteのサイズになるため、ここでは各パラメータ100個ずつ（100行分）のシーケンスデータが格納できることになります。

■各関節情報のパラメータ変数宣言

各関節を動かすための情報として、以下のパラメータを保存するメモリ領域を確保しています。

extern int G_STATUS;     /* 関節現在位置テーブル 24Byte分 */
extern int G_STPPOS;     /* 停止目標位置テーブル 24Byte分 */
extern int G_SPMODE;    /* 速度指定テーブル 24Byte分 */
extern char G_NRMODE; /* 正転/逆転テーブル　　 12Byte分 */

ここで確保している領域のサイズも、startup5.marの中のres.コマンドで指定しています。ここではchar型、int型ともに各々１２個分のパラメータが格納できる領域を確保しているため、１２関節分まで対応できます。

■PIOレジスタの定数宣言

今回、左足をPIOのポートAの8ビットで動かし、右足をPIOのポートＢの８ビットで動かします。

例えばこの先ハードウエアが変わり、別のPIOポートを使用することになった場合、プログラム中のソースファイルのPA.DR.BIT.B0などの記述を全て書き直さなければならなくなります。簡単なプログラムなら良いのですが、今回のように少し大きなプログラムになってくると大変な作業です。

そこで、PIOポートを示す別の定数を用意しておき、それはどのポートなのかを先頭で宣言しておくということをします。それを#defineによる定数宣言で行なっています。

例えば、左足太ももは以下のPIOポートのビットを利用しますが、

  PA.DR.BIT.B0

  PA.DR.BIT.B1

それを、以下の定数に置き換えて使用しています。

  P1_L0

  P1_L1

【先頭に戻る】

１５３−４−２．【２．メイン関数】 main()　の説明

メイン関数の前半では、ハードウエア環境の初期化と、各種変数の初期化を行なっています。その後、ハイパーターミナルと液晶表示器へのメッセージ表示を行い、主題となる受信コマンドごとの分岐処理に移ります。

受信コマンドによる分岐処理では、ハイパーターミナルから受信し１文字を判定して、次に実行させる関数を決定させています。

■Sコマンド　→　csv_conv( )関数を実行して、シーケンスデータテーブルの読み込みを行なう。

■Pコマンド　→　table_list( )関数を実行して、受信データテーブルを一覧表示させる。

■Lコマンド　→　pos_list( )関数を実行して、各関節の現在位置を一覧表示させる。

■Gコマンド　→　rb_move( )関数を実行して、シーケンスデータに基づいてロボットを動作させる。

それぞれの関数が終了したら、次のコマンド入力待ちの状態に戻ります。

【先頭に戻る】

１５３−４−３．【３．データ読み込み関数】 csv_conv( )　の説明

csv_conv( )関数では、パソコンで作成したCSV形式のテキストファイルを、文字データとして一気に読み込み、その後、【１．準備】の段階であらかじめ確保しておいたシーケンス用データテーブルのメモリ領域に、各パラメータごとに整理しながら格納して行きます。

■受信データの規則性

パソコンから受信するCSV形式のテキストファイルは、以下のフォーマットに決められていました。

コマンド（半角１文字），関節（２桁数字），速度（１桁数字），停止位置（２桁数字）　改行

例） E,01,1,16 (改行)

つまり、

はじめの1文字はアルファベット文字のコマンドで、

２文字目は区切りのカンマで、

３文字目と４文字目は関節番号を示す２桁の数字で、

５文字目は区切りのカンマで、

６文字目は速度を示す１桁の数字で、

７文字目は区切りのカンマで、

８文字目と９文字目は停止位置を示す２桁の数字で、

10文字目は改行を示すコード

であるという決め事です。

そして、続く１１文字目は、次の動作を示すデータテーブルの最初の一文字、つまり次の動作のためのコマンドであるということが分かります。

従って、ひとつの動作を示すデータテーブルは、１０文字のブロックで一塊になっていることが分かります。そして、何番目の文字に何の意味があるのか、も決められています。ここでは、この規則性に従って、受信したデータの整理を行なっています。

■受信データを一気に蓄える

先ず、rx_comm[ ]の文字配列変数に、受信したテキストファイルを一気に流し込みます。この動作は、Qという文字を受信するまで続けます。ここで注意。パソコンとAKI-H8は通信でハンドシェイクを行なっていないため、パソコンはAKI-H8の状態にお構いなしに、勝手にデータを送りつづけます。従って、AKI-H8側では、パソコンから一方的に送られてくるデータを取りこぼさないように、高速に受信処理を行なう必要があります。なので、ここの処理では、なるべく余計な仕事をさせないようにします。

■受信データを整理して、データテーブルに格納する。

rx_comm[ ]に蓄えた受信データを、上の規則性に基づいて整理してより分けます。はじめの１０文字分に含まれる各種パラメータと、不要なカンマ、改行コードを、以下のように整理します。

・コマンド１文字は、G_COMM　というメモリー領域の先頭位置に格納します。

・関節番号の２桁数字（文字）は、数値に変換してから　G_MOTOR　という領域の先頭に格納します。

・速度の１桁数字（文字）は、数値に変換してから　G_SPEED　という領域の先頭に格納します。

・停止位置の２桁数字（文字）は、数値に変換してから　G_POS　という領域の先頭に格納します。

・カンマや改行コードは使わないため、捨てます。

ところで、G_COMM , G_MOTOR , G_SPEED , G_POSの各メモリー領域を操作するため、*p_comm , *p_motor , *p_speed , *p_pos の各ポインタ変数を利用しています。

次の１０文字分に含まれる各種パラメータの格納場所は、これらのポインタ変数のアドレスを+1することによって指定し、順にメモリー領域を埋めていきます。

【先頭に戻る】

１５３−４−４．【４．データテーブル一覧表示関数】 table_list( )　の説明

table_list()関数では、既にG_COMM , G_MOTOR , G_SPEED , G_POSの各メモリー領域に格納されているデータを、*p_comm , *p_motor , *p_speed , *p_posの各ポインタ変数を使って順に読み出し、パソコンのハイパーターミナルに送信していきます。

このとき、G_COMM ( *p_comm )以外のパラメータは数値であるため、val_str_tx( )関数を使って数値を文字列に変換した後に送信しています。

ここでも、参照するデータテーブルを次々と繰り上げていくために、ポインタ変数のアドレスの+1を繰り返しています。

p_comm++;
p_motor++;
p_speed++;
p_pos++;

【先頭に戻る】

１５３−４−５．【５．関節位置一覧表示関数】 pos_list( )　の説明

pos_list()関数では、pos_scan（）関数で各関節の現在位置を取り込んでから、各関節位置を示すメモリー領域G_STATUSの内容を順に読み出して表示させます。

G_STATUSのメモリー領域を操作するために、*status というポインタ変数を利用しています。

pos_scan()関数では、0〜3チャンネルのA/D変換入力と4〜7チャンネルのA/D変換入力を順に取り込んでいます。ここでも*statusというポインタ変数を使い、status++;　とすることで、順にG_STATUSのメモリー領域を繰り上げながらデータを格納しています。

>>11 の部分は、10bitあるA/D変換データの上位5bitだけを利用するための処理です。詳しくは、１０４．Ａ／Ｄ変換機能を使うを参照して下さい。

【先頭に戻る】

１５３−４−６．【６．シーケンシャル動作関数】 rb_move( )　の説明

さて、ここからの処理が、このプログラムの主役です。ここでは、実際にロボットをシーケンシャルに動かして行きます。

rb_move（）関数では、以下の処理をループで高速に繰り返しています。ここではEコマンドの場合を例に取ります。

１．最新の関節位置を取り込む pos_scan()関数。

２．データテーブルのコマンドを判定する。

３．Eコマンドなので、すぐにデータテーブルの関節パラメータをセットするcomm_set()関数。関節パラメータはセットされた時点でPWM発生の割り込み関数で参照され、実際の関節の動作に反映される。

４．今セットしたパラメータを確認のためパソコン画面上に表示させるrun_disp()関数。

５．データテーブルを繰り上げ、次の動作を読み込む準備をしておく。

６．各関節の動作状態を監視して、停止させる必要があれば停止させる。move_ctrl()関数。

７．８個の関節の状態を一通り監視したら、動作中の関節数を戻り値end_flagに入力する。

８．シーケンスの終了条件を見て、ループ処理１．に戻る。

ここで、ループ処理１．に戻った時点では、５．の動作によって次のデータテーブルが参照されるため、２．では次の動作のコマンドが判定されます。

■もし、次のコマンドもＥコマンドだった場合。

すぐにパラメータセットの動作に入り、関節の動作が始まります。つまり、ひとつ前の動作とは、１回のループ時間分の時差しかないため、ほとんど同時に関節が動き出すことになります。

■もし、次がSコマンドだった場合。

Sコマンドの場合は、パラメータセットを開始するための条件として、end_flag == 0 が設定されています。つまり、move_ctrl()関数によって現在動いている関節が無いという判定結果が出るまで、パラメータセットが行なわれないことになります。

従って、ここでは何もしません。データテーブルの繰り上げもしないため、コマンドもSのままです。なので、move_ctrl()関数を実行して各関節の状態監視が一通り終わり、ループ処理１．に戻った時点でも、Sのままなので、再びend_flag == 0 という判定文を通過します。ここでは、end_flag が0になるまで、move_ctrl()関数による各関節の状態監視を高速に繰り返すことになります。

ここで、動作中の全ての関節が指定位置に達して停止すると、move_ctrl()関数の戻り値が0となり、ようやくパラメータセット動作に入ります。そしてその時点から関節が動き出します。で、データテーブルの繰り上げも合わせて行なわれるため、次のループ処理１．からは、次の動作のコマンドに合わせた動作が行なわれることになります。

■もし、次はWコマンドだった場合。

Wコマンドの場合も、Sコマンドと同様にend_flag == 0 の条件を入れてあります。もし、end_flagが0になった場合、ITU1のカウンタを動作させて割り込み処理を有効にします。ITU1では10mSごとに割り込みを発生させ、割り込み関数intimia1()内で WAITCNT というグローバル変数を+1させています。つまり、WAITCNTの数を数えると、Wコマンドが開始されてから何秒経過したかが10mS単位で計測できます。

ここでは、(*p_motor * 10) <= WAITCNT　という条件文によって、データテーブルで指定された時間が経過したことを判定しています。指定時間が経過した場合、パラメータセットは行なわずに、データテーブルの繰り上げと、WAITCNT変数の初期化をしてから、ループの最初に戻ります。

■もし、次はQコマンドだった場合。

Qコマンドの場合は、何もしません。ただし、ループ終了（シーケンス動作終了）の条件文が、(*p_comm != 'Q')||(end_flag != 0)　となっており、コマンドがQであり、end_flagも0になったらシーケンスが終了する、という動作になります。従って、Qコマンドの場合は、今動作中の関節が全て停止するまで待ってから、処理を終了させるということになります。

以上の動作は、常に高速ループによって各関節の動作を監視しつつ、コマンドに合わせたタイミングで、各関節のパラメータをセットして動作を制御しているということになります。そして、この制御は、８個の関節に対してほぼ同時に行なわれています。　

【先頭に戻る】

１５３−４−６−１．パラメータセット関数　comm_set()　の説明

comm_set()関数では、データテーブルで指定された関節のパラメータをセットします。ここで言う「セット」とは、各関節を制御するための G_STATUS , G_STPPOS , G_SPMODE , G_NRMODEの各メモリー領域の所定の場所に、パラメータに相当するデータを書き込むということです。

ところで、これらの各メモリー領域は、以下のように管理されます。

 G_STATUS , G_STPPOS , G_SPMODE , G_NRMODEの各々のメモリー領域には１２パラメータ分の格納領域があり、今回は８個の関節用のパラメータを上図のように格納します。

これらのパラメータの格納（セット）は、引数としてもらってくる、motor , speed, pos の各変数に基づいて行なわれます。

status = &G_STATUS + (motor - 1);
stppos = &G_STPPOS + (motor - 1);
spmode = &G_SPMODE + (motor - 1);
nrmode = &G_NRMODE + (motor - 1);

先ず、motor の値によって、各メモリー領域中の、所定の関節用のメモリーアドレスを指定します。G_STATUSを例に取ると、&G_STATUS は、G_STATUSというメモリー領域の先頭アドレス（関節01用）を示し、それに motor -1 を加えることによって、motor で指定される関節用のパラメータを格納するためのメモリーアドレスを示すことになります。そして、それを*statusというポインタ変数のポインタstatusに設定することによって、今後*statusという変数が、メモリーの所定の場所を操作するための変数になります。

例えば、motor の値が5だった場合、&G_STATUSが示すアドレスは01なので、これに5-1=4を足すと、01 + 4 つまり5となり、関節05用のパラメータを格納するアドレスを指定できることになります。

あとは同様にして、G_STPPOS , G_SPEED , G_NRMODE のメモリーの格納場所も、それぞれのポインタ変数で操作できます。

G_STPPOSには、引数のpos がそのまんまセットされ、

G_SPEEDには、引数のspeedがそのまんまセットされ、

G_NRMODEには、現在位置G_STATUSと目標停止位置G_STPPOSから、'+' 若しくは '-' の回転方向を示す文字がセットされます。

そして、このメモリー領域に書き込まれたパラメータは、即、PWM波形発生のためのITU2の割り込み処理で利用され、各関節の動作に反映されます。

【先頭に戻る】

１５３−４−６−２．関節動作監視制御関数　move_ctrl()　の説明

move_ctrl()関数では、G_STATUSにセットされている各関節の現在位置と、G_STPPOSにセットされている目標停止位置の比較を行い、同じならG_SPEEDのパラメータを0にして関節を停止させる動作を行い、行き過ぎていれば回転方向を示すG_NRMODEパラメータを逆の値に設定して、位置の補正を行ないます。

また、動作を継続している関節数を戻り値として返します。

この関数でも、各パラメータの操作にはポインタ変数を使用しています。

【先頭に戻る】

１５３−４−７．【７．PWM発生関数】 intimia2( )　の説明

intimia2()関数は、ITU2が5mSごとに発生するタイマー割り込みによって起動されます。そして、G_STATUS , G_STPPOS , G_SPMODE , G_NRMODEの各パラメータに基づいて、PWM波形を発生させます。

詳細については、１０７．タイマー割り込みでPWMするを参照して下さい。

【先頭に戻る】

１５３−５．プログラムの操作説明

	１．テキストエディタで、データテーブルを作成します。 作成したら、拡張子がTXTのテキストファイルとして保存します。
	２．次にハイパーターミナルを立ち上げます。 ハイパーターミナルの立ち上げ方法は、１０９．パソコンとシリアル通信するを参照して下さい。 ハイパーターミナルが立ち上がるリ、AKI-H8の電源を入れると、左図のようなメッセージが表示されます。
	３．半角大文字でSの文字を入力すると、左図のようなメッセージが表示され、ファイル読み込みモードになったことを示します。
	４．メニューバーの転送(T)から、テキストファイルの送信(T)を選択します。
	５．開いたダイアログボックスから、作成したデータファイルを指定します。
	６．ダイアログボックスから「開く」ボタンを押すと、読み込みが開始されます。 途中、データ処理の経過を示すメッセージが表示されます。これで、PLEASECOMMAND INPUTというメッセージが出て止まったら、データ読み込み終了です。
	７．半角大文字でPの文字を入力すると、今読み込んで処理されたデータを一覧表示します。 元のデータファイルと変わらないことを確認します。
	８．半角大文字でLの文字を入力すると、現在の各関節の位置を数値で一覧表示します。
	９．半角大文字でGの文字を入力すると、読み込んだデータファイルに基づいて、ロボットを動かします。 各データテーブルが実行されるたびに、実行されたコマンドとパラメータが表示されていき、作業の経過を表示します。 だけど．．．　なぜか69とか87とかいった変な数字が表示され、コマンドと関節番号が表示されない．．．。 これ、どうしても原因がわからなかったので、とりあえずこのまんま使ってます。すみません．．． ロボットの動作が全て終了すると　MOVING FINISHEDというメッセージが表示されます。

【先頭に戻る】

今回、シーケンシャル動作のタイミングを決めるコマンドとして、Ｅ、Ｓ、Ｗの３種類を用意しましたが、本当は、ＣとＴというのも作ろうと思ってました。

Ｃは、ある関節がある指定の角度に達したことを検出するためのチェックポイントを指定し、

Ｔは、そのチェックポイントを検出したタイミングで指定の関節の動作を開始させるというものです。

しかし、他の部分を作るのにちょっと手間取ったため、今回は見送りました。次の機会には組み込みたいと思います。

今回、シーケンシャル動作のプログラムを作って思ったのですが、単なるシーケンシャル動作では、歩行は無理かなあー、という感じがします。左右の足を同時に動かしても、左右の個体差によって関節の動作速度が違い、例えばロボットを屈伸させるためのシーケンスを組んでも、片ヒザだけが早く折れ曲がってしまい、見るからに「片ヒザを着きながら絶望のために倒れ込む人」のようになってしまったりします。（まさに今の力弥の心境．．．）

ここでは、両脚の動作状態をうまく同期させて、同じように関節が動くフィードバック処理なども必要になってきます。そういった意味では、パルス幅によって回転角度を制御できるサーボモーターや、与えるパルスの数で回転角度を制御するステッピングモーターよりも、ちょっと制御が厄介かなあー、という気がします．．．。(- -;

ソフトウエア編ＴＯＰに戻る】