ロビ２の姿勢制御
2024年1月6日
ロビの姿勢は主に直立、座り、充電椅子の３つの姿勢がありますがどの姿勢の時でもプログラムを動かす事ができます。
中には動かせないプログラムもありますがその姿勢に応じたモーションでプログラムが動きます。
ロビ１のプログラムは各姿勢に対するモーションがプログラムされています。
加えて顔の向きも左、正面、右と３方向あるのでその組み合わせで最大９パターンのモーションがプログラムされています。
ロビ２の場合プログラムで書かれているモーションは基本的に１パターンのみ(直立正面向き)です。
顔の向きはこちらの方法で顔が向いたままプログラムを実行させたりプログラムの開始で正面を向けて終了時に向いていた方向に戻す制御をしています。
姿勢に関してはプログラムでサーボを動かす時にマッピングテーブルを切替えて各姿勢に対してモーションがちゃんと動くように制御しています。
今回は実際にロビ２のプログラムでどのようにこの制御がされているかを解説しています。
サーボ制御のマッピング
プログラムのモーションでサーボを動かす時各サーボの位置(角度)とトルク(%)をマイコンボードメモリに書き込んでサーボを動かす時間をメモリに書き込むとその時間で全サーボが書込まれた位置に動きます。
位置とトルクを書込むメモリアドレスは0x0a00から始まる128バイトの領域でポーズデータとして定義します。
このポーズデータを連続して実行する事によりモーションしとして一連の動作をさせる事かできます。
ひとつのサーボの位置とトルクは2バイトずつの4バイトで定義されます。
0x0a00はサーボID1に対するデータですがサーボID1は未使用なので0x0a04からサーボID2(右足股横回転)の定義が始まります。
サーボID21(左手ヒジ)まで20個のサーボに対しては0x0a04〜0x0a52に書き込みます。
なおLEDに対してもモーションデータで点滅させています。
メモリアドレス0x0a70から左目LEDのR(赤)、G(緑)、B(青)、右目LEDのR、G、B、口LEDでそれぞれ2バイトずつ0〜255の値で定義されます。
LEDのデータは2×7バイトで0x0a70〜0x0a7cに書込みサーボと同時に指定した明るさにします。
サーボやLEDのデータを書き込む領域のアドレスは固定ではなくプログラムで設定されています。
STARTUP.BINの最初の部分で各サーボモーターの原点位置などのパラメーターをマイコンボードにセットしていますがそこでこのアドレスも定義されています。
0x00000104
<mem_w size="2" adr="0x1040">
0x0d82,0x0c04,0x0c06,0x0d02,0x0100,0x0019,0x8001,0x7fff,0x0000,・・・・・
この命令はサーボID2に対するパラメーターの設定で2番目の0x0c04がサーボ位置を書き込むアドレスで0x0c06がトルクを書き込むアドレスを設定しています。
実際のプログラムではサーボID2のサーボ位置は0x0a04に書き込むのでここで指定する0x0c04とは0x0200違ってきますがそういうものだと解釈してください。
0x00000a19
<mem_w size="2" adr="0xe20">
0x0c70,0x0c72,0x0c74,0x0c76,0x0c78,0x0c7a,0x0c7c,・・・・・・
この命令はLEDの明るさを書き込むアドレスを設定しています。
サーボと同様0x0200違っていますので0x0a70から0x0a7cに定義しています。
姿勢判定
ロビを起動したら初期化処理でロビの姿勢判定を行い各姿勢に応じたアイドリングポーズとサーボ制御のマッピングを行います。
このマッピングにより直立姿勢以外でモーションプログラムを動かしても動かしたくないサーボは動かないようになっています。
ロビの起動時の姿勢は直立、座り、充電椅子の３つの姿勢で起動時の手足の角度によって判定します。
姿勢判定は一連の初期化処理でサブルーチンをコールして行います。
0x0000010ec
<call adr="0x00004411"/>
次の手順で姿勢判定を行っています。
・右足の股縦(サーボID3)が前に曲がっていないか(サーボの角度が-60°より大きい)
上の条件を満たしている場合は立ち姿勢(戻り値0)
上の条件を満たしていない場合は下の判定を行う
・右足のヒザ(サーボID4)が曲がっているか(サーボの角度が-22.5°より大きい)
上の条件を満たしている場合は充電椅子姿勢(戻り値2)
上の条件を満たしていない場合は下の判定を行う
・左足のヒザ(サーボID9)が曲がっているか(サーボの角度が-17°より小さい)
上の条件を満たしている場合は充電椅子姿勢(戻り値2)
上の条件を満たしていない場合は座り姿勢(戻り値1)
すなわち右足が前方向に曲がっていない場合は直ち姿勢、右のヒザか左のビザかどちらか曲がっている場合は充電椅子姿勢でそれ以外は座り姿勢と判断しています。
0x000010f1
<calc><mem_r size="1" adr="0x0fa8"/><mem_w size="1" adr="0x0f82"/></calc>
姿勢判定から戻ると戻り値(0x0fa8)を姿勢変数(0x0f82)にセットします。
0x000010fc
<calc><mem_r size="1" adr="0x0f82"/><const data="0"/><sub/><eq/><jump adr="0x0000e03"/></calc>
姿勢変数(0x0f82)が0(立ち姿勢)の場合は立ち姿勢のサーボ制御処理にジャンプします。
0x000010fc
<calc><mem_r size="1" adr="0x0f82"/><const data="1"/><sub/><eq/><jump adr="0x0000e9b"/></calc>
姿勢変数(0x0f82)が1(座り姿勢)の場合は座り姿勢のサーボ制御処理にジャンプします。
0x000001020
<jump adr="0x00000f33"/>
姿勢変数(0x0f82)が0,1以外の場合は充電椅子姿勢のサーボ制御処理にジャンプします。
0x00000e03
<mem_w size="2" adr="0x0a00">
0x0000,0x0000,0x0000,0x0050,0x0000,0x0050,0x0000,0x0050,0x0000,0x0050,・・・
立ち姿勢のアイドリングポーズ(全てのサーボが原点)を書き込みます。
0x00000e9b
<mem_w size="2" adr="0x0a00">
0x0000,0x0000,0xffe2,0x0050,0xfbd2,0x0050,0x0000,0x0050,0xfef2,0x0050,・・・
座り姿勢のアイドリングポーズを書き込みます。
0x0000130c
<mem_w size="2" adr="0x0ac0">
0x0000,0xffe2,0xfbd2,0x0000,0xfef2,0x0000,0x001e,0x042e,0x0000,0x010e,・・・
マッピングするメモリアドレスにも座り姿勢のアイドリングポーズを書き込みます。
0x000011f7
<mem_w size="2" adr="0x1042">0x0cc2</mem_w>
<mem_w size="2" adr="0x1044">0x0cea</mem_w>
マッピングするサーボモーターのパラメーターをマッピングするアドレスに書換える
ここで書き換えるサーボはID1(ダミー),ID2〜ID11(両足),ID12(腰),ID16〜ID21(両手)のパラメータ―
トルクのマッピングアドレスはID1(ダミー),ID2〜ID11(両足)は0x0ceaでID12(腰),ID16〜ID21(両手)は0x0ceeで0x0ceaの値は0x46(70%)、0x0ceeの値は0x32(50%)が書かれている。
0x00000f33
<mem_w size="2" adr="0x0a00">
0x0000,0x0000,0xffe7,0x0050,0xfc4a,0x0050,0x0096,0x0050,0xff60,0x0050,・・・
充電椅子姿勢のアイドリングポーズを書き込みます。
0x00001364
<mem_w size="2" adr="0x0ac0">
0x0000,0xffe7,0xfc4a,0x0096,0xff60,0x0000,0x0019,0x03b6,0xff6a,0x00a0,0xfff1,・・・
マッピングするメモリアドレスにも充電椅子姿勢のアイドリングポーズを書き込みます。
0x000013c7
<mem_w size="2" adr="0x1042">0x0cc2</mem_w>
<mem_w size="2" adr="0x1044">0x0cea</mem_w>
マッピングするサーボモーターのパラメーターをマッピングするアドレスに書換える
ここで書き換えるサーボはID1(ダミー),ID2〜ID11(両足)のパラメータ―
トルクのマッピングアドレスは0x0ceaで値は0x46(70%)が書かれている。
なお充電椅子姿勢から他の姿勢への遷移はできないのでここでプログラム終了までこの状態で動きます。
座り姿勢への遷移
立ち姿勢から座り姿勢への遷移は「座って」などの命令でロビが座る動作が終了した時に行われます。
0x0000718b
<calc><const data="1"/><mem_w size="1" adr="0x0f82"/></calc>
姿勢変数(0x0f82)を座り姿勢(1)にする
0x00007196
<mem_w size="2" adr="0x0a00"/>
0x0000,0x0000,0xffe2,0x0050,0xfbd2,0x0050,・・・
座り姿勢のポーズにする
0x00007233
<call adr="0x0002edce"/>
マッピングアドレスを切替えるルーチンをコールする
0x0002edce
<jump adr="0x0002eef6"/>
0x00002eef6
<calc><mem_r size="1" adr="0x0f82"/><const data="0"/><sub/><eq/><jump adr="0x0002ef0d"/></calc>
姿勢変数(0x0f82)が1(座り姿勢)の場合は座り姿勢のサーボ制御処理にジャンプします。
0x00002ef0d
<mem_w size="2" adr="0x0ac0">
0x0000,0xffe2,0xfbd2,0x0000,0xfef2,0x0000,0x001e,0x042e,0x0000,0x010e,・・・
マッピングするメモリアドレスにも座り姿勢のアイドリングポーズを書き込みます。
0x0002ede5
<mem_w size="2" adr="0x1042">0x0cc2</mem_w>
<mem_w size="2" adr="0x1044">0x0cea</mem_w>
マッピングするサーボモーターのパラメーターをマッピングするアドレスに書換える
ここで書き換えるサーボはID1(ダミー),ID2〜ID11(両足),ID12(腰),ID16〜ID21(両手)のパラメータ―
トルクのマッピングアドレスはID1(ダミー),ID2〜ID11(両足)は0x0ceaでID12(腰),ID16〜ID21(両手)は0x0ceeで0x0ceaの値は0x46(70%)、0x0ceeの値は0x32(50%)が書かれている。
立ち姿勢への遷移
座り姿勢から立ち姿勢への遷移は「立って」などの命令や立って行うプログラムの時ロビが立ち上がる動作を行う前に行われます。
0x00110a5c
<mem_w size="2" adr="0x0ac0">
0x0000,0xffe2,0xfbe6,0x0000,0xfef2,0x0000,0x001e,0x041a,0x0000,0x010e,・・・
なぜかマッピングしているメモリアドレスにも座り姿勢のアイドリングポーズを書き込んでいます。
0x0011096f
<mem_w size="2" adr="0x0a00">
0x0000,0x0064,0xffe2,0x0050,0xfbe6,0x0050,0x0000,0x0050,0xfef2,0x0050,・・・
元のメモリアドレスにも立ち姿勢のアイドリングポーズを書き込んでいます。
0x0011085a
<mem_w size="2" adr="0x1042">0x0c04</mem_w>
<mem_w size="2" adr="0x1044">0x0c06</mem_w>
マッピンしてあるサーボモーターのパラメーターを元のアドレスに書換える
ここで書き換えるサーボはID1(ダミー),ID2〜ID11(両足),ID12(腰),ID16〜ID21(両手)のパラメータ―
0x00110388
<mem_w size="2" adr="0x0a00">
0x0000,0x0064,0xffe3,0x0050,0xfc1b,0x0050,0x0497,0x0050,0xff0e,0x0050,・・・
立ち上がるモーションプログラムを実行する
0x0011083e
<calc><const data="0"/><mem_w size="1" adr="0x0f82"/></calc>
一連の立ち上がりモーション終了後姿勢変数(0x0f82)を座り姿勢(0)にする
ロビライドモードのサーボ制御
ロビ２の場合３つの姿勢以外にロビライドモードの姿勢制御を行っています。
ロビライドモードの姿勢は姿勢変数(0x0f82)にロビライドモード(3)がセットされます。
立ち姿勢からロビライドへの遷移は「ロビライドで遊ぼう」などの命令でロビがロビライドに乗る動作が終了した時に行われます。
0x00175f32
<calc><const data="3"/><mem_w size="1" adr="0x0f82"/></calc>
姿勢変数(0x0f82)をロビライドモード(3)にする
0x00175f28
<call adr="0x000281d00"/>
マッピングアドレスを切替えるルーチンをコールする
0x00281d00
<jump adr="0x00281e7b"/>
0x00281e7b
<calc><mem_r size="1" adr="0x0f82"/><const data="3"/><sub/><eq/><jump adr="0x00281d07"/></calc>
<return/>
姿勢変数(0x0f82)が3(ロビライド)以外の場合は何もせず戻る
0x00281d07
<mem_w size="2" adr="0x0ac0">
0x0000,0xfff6,0xff7e,0x0000,0xffb0,0xfffb,0x000a,0x0082,0x0000,0x0050,・・・
マッピングするメモリアドレスにもロビライドモードのアイドリングポーズを書き込みます。
0x00281d6a
<mem_w size="2" adr="0x1042">0x0cc2</mem_w>
<mem_w size="2" adr="0x1044">0x0cea</mem_w>
マッピングするサーボモーターのパラメーターをマッピングするアドレスに書換える
ここで書き換えるサーボはID1(ダミー),ID2〜ID11(両足),ID12(腰),ID16〜ID21(両手)のパラメータ―
トルクのマッピングアドレスはID1(ダミー),ID2〜ID11(両足)は0x0ceaでID12(腰),ID16〜ID21(両手)は0x0ceeで0x0ceaの値は0x46(70%)、0x0ceeの値は0x32(50%)が書かれている。
ロビライドモードから立ち姿勢への遷移は「ロビライドおしまい」などの命令でロビがロビライドから降りる動作で行われます。
0x001761ee
<call adr="0x00283693"/>
マッピングアドレスを切替えるルーチンをコールする
0x00283693
<jump adr="0x0028369a"/>
0x00281e7b
<calc><mem_r size="" adr="0x0f82"/><const data="3"/><sub/><eq/><jump adr="0x002836ad"/></calc>
<return/>
姿勢変数(0x0f82)が3(ロビライド)以外の場合は何もせず戻る
0x002836ad
<mem_w size="2" adr="0x0a00">
0x0000,0x0000,0xfff6,0x0050,0xff7e,0x0050,0x0000,0x0050,0xffb0,0x0050,・・・
元のメモリアドレスにも立ち姿勢のアイドリングポーズを書き込んでいます。
0x00283750
<mem_w size="2" adr="0x1042">0x0c04</mem_w>
<mem_w size="2" adr="0x1044">0x0c06</mem_w>
マッピンしてあるサーボモーターのパラメーターを元のアドレスに書換える
ここで書き換えるサーボはID1(ダミー),ID2〜ID11(両足),ID12(腰),ID16〜ID21(両手)のパラメータ―
0x00283890
<mem_w size="2" adr="0x0e20">0x0c70</mem_w>
LEDのマッピンアドレスも元に戻している(LEDのマッピングアドレスは変えていないが)
0x001761f8
<calc><const data="0"/><mem_w size="1" adr="0x0f82"/></calc>
姿勢変数(0x0f82)を立ち姿勢(0)にする
ロビライドのプログラムはロビ１と共通なため立ち姿勢のマッピングで書かれているものが多くこれらのプログラムは実行する前に立ち姿勢のマッピングに戻すルーチンをコールしています。
プログラムが実行した後にロビライドのマッピングに切替えるルーチンをコールしています。
これらのルーチンはモードの遷移の時に呼ばれたものと共通です。
例として認識語「ロビライド説明して」の流れを示します。
0x00284191
<calc><mem_r size="" adr="0x0e5c"/><const data="472"/><sub/><eq/><jump adr="0x00284191"/></calc>
認識語ロビライド説明して(472)でジャンプする
0x0028419b
<call adr="0x003fe7b8"/>
ロビライド説明してのプログラム(サブルーチン)をコールする
0x00400494
<call adr="0x00283693"/>
マッピングアドレスを切替えるルーチン(モード遷移と共通)をコールする
0x00400400
<call adr="0x00281d00"/>
一連のプグラム終了後マッピングアドレスを切替えるルーチン(モード遷移と共通)をコールする
STARTUP.BINにパッチ(充電椅子で直立モード.csv)を当てて充電椅子姿勢で起動した時充電椅子姿勢のサーボ制御を行った後、姿勢変数(0x0f82)に立ち姿勢(0)を入れるよう変更してみました。
各プログラムの頭でこの姿勢変数を見て充電椅子姿勢だと動かないプログラムがあくつかありますがこのパッチで強制的に動かす事ができます。
ただしロビを命令で座らせたりタイムアウトで座らせると立ち上がる事があるので注意してください。
またロビ１のダンスの前後でロビ２と同じサーボ制御をするようにしてロビ２の同様に足のサーボが動かなくなるか試してみました。
ロビ2のパッチファイルとロビ1の置き換えプログラムは以下のファイルをダウンロードしてください。
chair_mode.zip
なおロビのプログラムにパッチを当てたりプログラムを置き替える時は必ずバックアップを取って自己責任で行ってください。
目次に戻る