2001/06/24 【ソフトウエア編ＴＯＰに戻る】
さて、ロボットの足腰ができたところで、取りあえず好きな関節を好きなだけ動かせる制御ソフトを作ってみましょう。このソフトを作っておくと、ロボットのメンテナンスや、バランスの取り方などの実験に役立ちます。
AKI-H8開発ボード上にはスイッチ類が幾つか付いているので、それを利用してリモート制御させることも可能でしょうが、今回はより細かい指示を与えるために、パソコンのハイパーターミナルからコマンドを入力して動かすことにします。
ただし、今回のソフトはモーター８個までの制御に対応です。全部で１２個ある足腰のモーター全てには対応していません。
１５２−１．ロボット制御のコマンド
ここで言うコマンドとは、ロボットに対する命令文という意味です。それをハイパーターミナルから入力して、ロボットに与えてあげます。今回は、以下のようなコマンドを考えました。
| 文字順 | 入力値 | 説 明 |
| 1 |
L,R （大文字のみ） |
左右動かしたい足を指定する。
LL：左足、RR：右足、LR：左右同時 |
| 2 | ||
| 3 | 1,2,3,4 |
動かしたい関節を指定する。 1：太もも、2：ヒザ、3：足首、4：ガニマタ |
| 4 | 0,1,2,3,4,5,6,7,8 |
モーターの回転速度を指定する。 0：停止、1：（低速低トルク）〜8：（高速高トルク） |
| 5 | S,Z (大文字のみ） |
停止位置の設定モードを指定する。 S：相対位置指定、Z：絶対位置指定 |
| 6 | +,- |
相対位置指定の時の、関節回転方向を指定する。 +：正回転方向、-：逆回転方向 |
| 7 | 00〜31 |
停止させる関節位置を指定する。 00〜31の32段階。ただし、制限も持たせる。 |
| 8 |
入力する文字は全て半角文字です。また、間違ったコマンドに対する診断機能は持たせていません。
■コマンド例
例１） LL23S+05
左ヒザの関節を、３の回転速度で、現在位置から正方向に５ポイントだけ動かす。
例２） LR35Z+16
左右の足首を、５の回転速度で、絶対位置１６の角度まで動かす。
■補足説明
相対位置指定とは、現在の関節角度を基点として、関節を正方向/逆方向に指定角度だけ動かしたい、という意味です。これに対して絶対位置指定とは、現在の関節角度は関係なく、直接指定する関節角度まで動かすということになります。なので、次の+,-の指定は無視されますが、必ず+か-は入力しておいて下さい。
関節角度は00〜31の３２段階で指定します。これは、各関節に付けてあるポテンショメータから得られる値を３２段階に分けたものなので、約２７０度を３２段階に刻んだ角度となります。（全然精度が低いことがバレてしまいますね．．．）
どの関節位置の時に、ポテンショメータを中間位置にしておくか、といった、関節ごとの細かい調整が必要になりますが、そのあたりは別途アクチュエータ編でご紹介することにします。
コマンドが長くてわずらわしいのですが、カンベンして下さい。m(_ _;m
■その他の機能
１．コマンド入力のし直し処理
コマンド入力の途中で始からやり直したいとき、Qのキーを押すと、最初から入力し直せます。
２．各関節の現在位置の一覧表示
ロボットの各関節の、現在位置（関節角度）が知りたい時、Mのキーを押すと、一覧で表示されます。
■実際の使用例
実際の使用例と、操作説明、そして、ロボットの各関節の動作などについては、アクチュエータ編の９．各関節角度とポテンショメータの調整を参照して下さい。
１５２−２．プログラム全体の流れ
このプログラム rbtest1.c は、大体以下の流れで動作します。なお、今回のプログラムは、このページ上に掲載しません。（だらだらと長すぎるため） プログラムソースrbtest1.cは、ここをクリックするとダウンロードできますので、参考にして下さい。
なお、スタートアップオブジェクトは、今回割込みを利用しているので、startup4.marでないと動作しません。startup4.marはstartup.marに名前を変更してから、ソフトウエア編の３．コンパイル手順 ３−１．スタートアップオブジェクトを作るを参照して、startup.objファイルを作成し、rbtest1.objとリンクして下さい。
|
|
■各部の説明にジャンプします。 |
次に各部の動作を簡単にご紹介します。
１５２−３．プログラムの説明
■ヘッダファイルの読み込み
今回のプログラムでは、おなじみのヘッダフィル3048f.hの他に、今まで自分で作ってきた汎用的な関数を収めたmyfunc.hも利用します。myfunc..hについては２０１．自分の関数をヘッダファイルにするを参照して下さい。
■割込み関数の宣言
今回、PWM波形を発生させるため、ITU2のタイマー割込みを利用しています。なので、#pragma interrupt(intimia2)として、intimia2()の関数を割込み関数として宣言しています。
■各関節情報のパラメータ変数宣言
各関節を動かすための情報として、以下のパラメータを関節ごとに持たせています。
STATUS：各関節の現在位置を示す。(0〜31)
SPMODE：各関節を動かす時の回転速度を示す。(0〜8）
STPPOS：各関節を動かす時、停止させる目標位置を示す。(0〜31）
これらのパラメータは、各関節ごとに持たせるため、例えばSTATUS1Lというように、パラメータの後に関節を示す数字と、左右の足を示すL/Rを付けて、各関節のパラメータ変数としています。
■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
メイン関数では、先ず最初に各種変数の宣言や、ハードウエア環境の初期化を行なっています。
char command[10]; /* 受信コマンド */
は、パソコンから受信するコマンド文字列を格納するための、文字配列変数です。
その後、タイマーや液晶表示器、PIOポートの初期化を行い、PWMを発生させるITU2の初期化も行なっています。
CNT = 0;
は、PWMを発生させる上で割込み回数を数えるためのグローバル変数です。ITU割込み処理によるPWM発生については、１０７．タイマー割込みでPWMするを参照して下さい。
SPMODE1L = 0;
SPMODE2L = 0;
SPMODE3L = 0;
SPMODE4L = 0;
SPMODE1R = 0;
SPMODE2R = 0;
SPMODE3R = 0;
SPMODE4R = 0;
では、各関節の動作速度パラメータを0（ゼロ）つまり停止として初期化しています。起動したばかりのプログラムでは、変数の中身に何が入っているか分からないので、初期化が必要です。例えば上記のように初期化しない場合、プログラム実行と共に各関節がバラバラに動き出してしまいます。
/*パソコンと液晶画面表示*/
menu_write();
パソコンと液晶表示器にメッセージを表示させます。パソコンには、簡単な制御コマンドの説明を表示させることによって、マンマシンインターフェイスが向上されます。
/*ロボット駆動制御 */
while(1){
P5.DR.BYTE = 0x01; /* 通常処理期間のLED表示*/
comm_in(command); /* コマンド文字列の受信 */
pos_scan();
/* 関節位置読み込み */
comm_set(command); /* コマンド文字列でパラメータを設定する */
mo_ctrl(command); /*モーター状態監視制御*/
}
ここからがメイン関数の主な処理部分となります。この辺の流れは、上でご紹介しているフローチャートそのものです。各関数については、以下にご説明します。
１５２−３−３．【３．コマンド゛入力関数】comm_in(command)の説明
ここでは、パソコンのハイパーターミナルから送られてくる文字をcommand[ ]変数に格納して行き、リターンキーが押されるまで繰り返しています。
rx_data = sci1_rx(); /* 1文字受信 */
で、受信した文字を一時的にrx_dataという変数で受け取ります。
if(rx_data != '\r'){ /* リターン以外を受信した時の処理*/
sci1_tx(rx_data); /* 1文字送信 */
lcd_write4(rx_data,1); /* 1文字液晶表示 */
rx_comm[i] = rx_data; /* 1文字を文字列に格納 */
i++;
}
ここでは、受信した文字がリターンキー以外の時、パソコンと液晶に受信した文字を送信して、rx_comm[ ]という文字配列変数に格納して行きます。
if((rx_data == 'Q')||(rx_data == 'q')){ /*入力し直し処理*/
i = 0;
sci1_tx('\r');
sci1_strtx(" "); /* パソコン表示クリア*/
sci1_tx('\r');
lcd_write4(0x01,0); /* 画面クリア */
lcd_locate(0,0); /* 液晶表示位置指定1行目先頭*/
}
ここでは、Qかqを受信すると、コマンドを始から受信しなおします。パソコンと液晶の画面をクリアして、i = 0;としています。ここで、i は受信している文字の番号を管理しているため、i = 0;とすることで「次に受信する文字がコマンドの１文字目だよ」ということになるのです。
if(rx_data == 'M'){
/*各関節位置の一覧表示*/
pos_scan(); /*関節位置読み込み */
sci1_tx('\r');
sci1_tx('\n');
sci1_strtx("STATUS1L = "); val_str_tx(STATUS1L);
sci1_strtx("STATUS2L = "); val_str_tx(STATUS2L);
sci1_strtx("STATUS3L = "); val_str_tx(STATUS3L);
sci1_strtx("STATUS4L = "); val_str_tx(STATUS4L);
sci1_strtx("STATUS1R = "); val_str_tx(STATUS1R);
sci1_strtx("STATUS2R = "); val_str_tx(STATUS2R);
sci1_strtx("STATUS3R = "); val_str_tx(STATUS3R);
sci1_strtx("STATUS4R = "); val_str_tx(STATUS4R);
sci1_tx('\r');
sci1_tx('\n');
sci1_tx('\r');
sci1_tx('\n');
i = 0;
}
ここでは、Mを受信すると、各関節の現在の位置をパソコンに一覧で表示させます。
pos_scan(); /*関節位置読み込み */
が関節位置の取込関数です。後でご説明します。ここでもi = 0;としています。従って、この後コマンド入力は始めっからやり直すことになります。
rx_comm[i] = '\0';
リターンが入力された後、コマンド文字列の締めくくりとして最後に'\0'を入れておきます。これは文字列の最後を示します。
/*入力したコマンド文字列を引数*commandに渡す*/
for(i=0;i<10;i++){
*command = rx_comm[i];
command++;
}
最後に、引数*commandにデータを渡してあげます。始めっから*commandに対して直接データの処理を行なうこともできますが、*commandのポインタ変数を扱うより、rx_comm[ ]の配列変数を扱ったほうが処理が楽なので、最後に*commandにデータを渡してあげることにしました。
１５２−３−４．【４．関節位置取込関数】pos_scan()の説明
ここでは、A/D変換機能の0〜3チャンネルスキャンと、4〜7チャンネルスキャンを行って、各関節についているポテンショメータの回転角度を取り込んでいます。
pos_scan(void){
/*A/D変換設定 SCANMODE ch0-3 */
AD.CSR.BYTE = 0x33;
while(AD.CSR.BIT.ADF == 0){}
/*スキャン停止*/
AD.CSR.BIT.ADST = 0;
/*現在位置取り込み*/
STATUS1L = AD.DRA>>11;
STATUS2L = AD.DRB>>11;
STATUS3L = AD.DRC>>11;
STATUS4L = AD.DRD>>11;
先ず始に、左足の４個分の関節角度を取り込みます。
AD.CSR.BYTE = 0x33;
は、0〜3チャンネルのアナログ入力をスキャンモードで取り込む設定です。
while(AD.CSR.BIT.ADF == 0){}
で、スキャン動作が終了するまで時間を潰しています。
AD.CSR.BIT.ADST = 0;
で、スキャン動作を停止させておきます。
STATUS1L = AD.DRA>>11;
STATUS2L = AD.DRB>>11;
STATUS3L = AD.DRC>>11;
STATUS4L = AD.DRD>>11;
では、とれぞれ取り込んだアナログ値のデータを１１ビット右にシフトさせ、５ビットの分解能（32段階）まで精度を落としてから、各STATUSパラメータに格納しています。
次に右足の４個分の関節角度を取り込みます。
動作は左足と全く一緒です。ただし、
/*A/D変換設定 SCANMODE ch4-7 */
AD.CSR.BYTE = 0x37;
として、4〜7チャンネルのアナログ入力をスキャンするように設定しています。
A/D変換の詳細に付きましては、１０４．A/D変換機能を使うを参照して下さい。
１５２−３−５．【５．パラメータセット関数】comm_set(command)の説明
ここでは、受信したコマンド文字列を元に、各関節のパラメータSTATUS,SPMODE,STPPOSの値をセットして行きます。
ソースコード自体はだらだらと長いため、流れだけを簡単にご説明します。以下の手順でコマンドを解析して条件を絞り込んでいきます。
１．コマンドで左右のどちらの足を動かすかを判定する。
コマンドは LL：左足、RR：右足、LR：左右同時 という意味でした。なので、送られてきたコマンドの１文字目がLの場合は左足用のパラメータセット、２文字目がRの時は右足用のパラメータセット、という意味になります。この条件だけ見ておけば、左右個別に動かしたい時と、左右同時に動かしたい時の両方に適用することができます。
２．コマンドでどの関節を動かすかを判定する。
コマンドでは1：太もも、2：ヒザ、3：足首、4：ガニマタでした。つまり、コマンド文字列の3文字目を判定することによって、どの関節のためのコマンドなのか、が分かります。この時点で、関節の回転速度を設定するSPMODEのパラメータを確定することができます。
３．停止位置の指定は相対か絶対かを判定する。
コマンドではS：相対位置指定、Z：絶対位置指定でした。つまり、コマンド文字列の5文字目を判定することによって、どちらのモードかが分かります。
相対位置指定の場合、更に回転方向+か-かをコマンド文字列の6文字目から判定し、関節の現在位置STATUSパラメータを基点にして、コマンド文字列7,8文字目の移動量を足したり引いたりすると、停止の目標位置STPPOSパラメータを算出することが出来ます。
絶対位置指定の場合は、コマンド文字列7,8文字目の数字が、そのままSTPPOSパラメータの値となります。
ここで注意が必要なのが、コマンド文字列7,8文字目は、数字ではなく文字であるためそのままでは計算に使えないということです。(comm[6] & 0x0F)*10)+(comm[7] & 0x0Fでは、その２文字を2桁の数字に変換しています。
これらの判定と、各パラメータへの値の設定を、全ての関節について行なえるように条件文を記述しています。
なお、ここでSPMODEパラメータが書き換えられると、それは即、PWM信号発生の割り込み関数に反映されます。つまり、ロボットが動き出すことになります。
１５２−３−６．【６．関節状態監視制御関数】mo_ctrl(command)の説明
ここでは、パラメータセット関数によって動き出したロボットの関節角度を常に監視し、関節角度が目標の停止位置に達したら関節の動きを停止させる働きをしています。
関節が動くと、STATUSパラメータは常に変化します。なので、mo_ctrl()関数では、do{ }while文によって、関節が目標位置STPPOSパラメータに達するまで繰り返し関節角度を取り込み、目標位置と現在位置の比較監視を繰り返しています。
関節が目標位置に達すると、SPMODEパラメータを0（ゼロ）にして、PWM信号の発生を停止させます。
この関数は現在のところまだ充分ではありません。この関数を拡張して行く事によって、以下のような処理を行なうことができると考えています。
１．行き過ぎを修正する処理
現在のところ、現在位置が目標位置を横切っただけで関節を停止させています。つまり、勢いあまって行き過ぎた分を戻したりして補正する機能がありません。プログラムの追加によって、現在位置が目標位置と一致するまで修正を繰り返し、精度の高い制御を行なうことが出来るようになるでしょう。
２．かける力を修正する処理
例えば、コマンドで指定した回転速度（回転トルク）では、トルクが不足して関節を動かせない場合、動作開始から経過した時間と、関節が移動した量の情報から、力不足で動かないという状況を認識することができます。その場合は、自動的にSPMODEパラメータを増やしていき、関節が動くように修正させるようにできると思います。
３．スムースな停止処理
先ほどの１．と関連しますが、コマンドの通りに目標位置まで勢い良く動かすのではなく、目標位置の数ポイント前になったら、少しずつ回転速度を緩めていくといった処理をする事によって、スムースな停止が行なえるようになると思います。
４．関節の可動範囲の制御
目標位置は0〜31の値で指定できますが、各関節によって動かせる範囲は構造的に決まってしまいます。構造的ぶつかってしまうのに無理に動かそうとすると、ボディやモータが破損してしまう恐れもあります。なので、各関節ごとに可動範囲を設定してあげて、現在位置がその範囲を超えた場合は、SPMODEパラメータを0（ゼロ）にするという処理も必要です。ただし、今回この処理は、PWM発生関数に含めています。
１５２−３−７．【７．PWM発生関数】intimia2()の説明
ここでは、メイン関数からの一連の動作フローからは完全に独立して、ITU2で5msごとに発生する割り込み処理によって、各関節を動かすためのPWM信号を発生させています。
処理内容の概要は以下の通りです。
１．関節可動範囲の設定
各関節での可動範囲を超えた目標値STPPOSパラメータが設定された場合、SPMODEパラメータを0（ゼロ）にして、動作させないようにする。現在、可動範囲の細かな設定はしていません。一律で4〜27の範囲を可動範囲として設定しています。
２．停止処理
SPMODEパラメータが0（ゼロ）の場合、該当する関節の制御２ビット分を両方とも1に設定して、関節が動作しないようにしています。ちなみに、両方とも1に設定するとブレーキモードになります。
３．各関節ごとのPWM信号発生
各関節のSPMODEパラメータに基づいたデューティー比のPWM波形を発生させます。PWM波形はITU2の割り込み発生回数をCNT変数でカウントすることによって、指定のデューティー比を得ています。詳細については、１０７．タイマー割り込みでPWMするを参照して下さい。
今回のプログラムのソースコードは、余り人様にお見せできるものではありません。だらだらと長く、いかにも力ずくで作ったというのが見え見えです。(^^; その原因の一つとして、各関節のパラメータSTATUS , SPMODE , STPPOSを配列変数にしていない、というのが上げられます。
例えば、STATUS[i]といった感じにしておけば、全ての関節の処理を i という変数を変えてあげるだけで、同じソースコードで処理することが出来るようになります。今回はSTATUS1LとかSTATUS2Lといったように、全く別個の変数にしているために、ソースコード上も全く同じ内容の処理の記述を、関節の数だけ並べています。
では、なぜ配列変数にしなかったのか？それは力弥の力不足によるものです。(^^;
今回、各パラメータは、PWM発生の割り込み関数上でも利用するため、グローバル変数にする必要がありました。その場合、startup.marでも同じ変数を宣言してメモリー領域の確保をする関係上、startup.marと共通で使える配列変数というのをどうすれば良いのかが分からなかったのです。
やり方としては、startup.mar上に、全てのパラメータ分のメモリー領域を、ある変数名で確保しておき、後は、Ｃ言語側で作業用に用意した配列によるパラメータ変数で、うまくメモリー領域を操作していくということになるかと思います。つまり領域を確保した変数のポインタと、作業用の配列変数とのやり取りということになります。
そもそも、H8-3048/FのCPUにPWM出力が5チャンネルまでしかないので、プログラム的に割り込みでPWMを発生させているのが根本にある訳けです。ちょっと恨めしい気持ちもしますが、もう少しの間はH8を見放さないで、出来る限り使っていこうかと思います。
rbtest1.cをコンパイルして得られたrbtest1.motファイルは、約21KByteでした。AKI-H8のROM容量は128KByteなので、まだ余裕がありますが、今のうちからメモリー容量を節約できるプログラミングを意識していきたいとは思っています。
更新記録
2001/06/25 アクチュエータ編９．各関節角度とポテンショメータの調整とのリンク追加