只今編集中です。
モーションでFREEDOM jr.IIIを動かしてみましょう。 モーションを再生する関数はたった1行です。 1行だけとは言ってもそのために準備が必要です。 準備はどのプログラムでも大体同じですから、まずは深く考えずコピペ(コピー&ペースト)します。
以下のソースを元に解説します。
#include <fd.h> #include "share/Humanoid16Axis.h" #include "share/HumanoidServoParam.c" #include "share/HumanoidSampleFlashData.c" TApplyPart WholeBody = { Priority: 1, PartNum: 16, part:{ &((THumanoid*)0)->Servos.RightLeg[0], &((THumanoid*)0)->Servos.RightLeg[1], &((THumanoid*)0)->Servos.RightLeg[2], &((THumanoid*)0)->Servos.RightLeg[3], &((THumanoid*)0)->Servos.RightLeg[4], &((THumanoid*)0)->Servos.LeftLeg[0], &((THumanoid*)0)->Servos.LeftLeg[1], &((THumanoid*)0)->Servos.LeftLeg[2], &((THumanoid*)0)->Servos.LeftLeg[3], &((THumanoid*)0)->Servos.LeftLeg[4], &((THumanoid*)0)->Servos.RightArm[0], &((THumanoid*)0)->Servos.RightArm[1], &((THumanoid*)0)->Servos.RightArm[2], &((THumanoid*)0)->Servos.LeftArm[0], &((THumanoid*)0)->Servos.LeftArm[1], &((THumanoid*)0)->Servos.LeftArm[2] } }; void main (void) { fd_Init (115200, false, _MONITOR_VOLTAGE); fd_SetServoParameter (ConvToPPose (&HomePos), SVParam, fd_SVSize(SVParam)); fd_SetMotion (ConvToPPose (WakeUp), 1, fd_MSize(WakeUp), 100, &WholeBody); while(1); }
ここで言うモーションとは、ロボットの動作のことです。手を振ったり、お辞儀したり、起き上がったり、FREEDOM jr.IIIの動きはほとんどモーションです。
モーションはよくペラペラ漫画に例えられます。ペラペラ漫画をもっと荒く(コマ数を少なく)したものです。例えばお辞儀するモーションなら3コマでできます。「直立姿勢」->「お辞儀した姿勢」->「直立姿勢」です。それを繋げてお辞儀する動作になります。
[お辞儀の画像]
大抵ロボットでモーションを再生するとロボットダンスのようにカクカクした動きになります。しかしFREEDOM jr.IIIは、コマとコマの間を補間して加減速することで滑らかな動きを実現しています。
[モーション再生の動画]
サンプルソースを中身を見て行きましょう。
モーションを再生するためには先ずモーションを再生する関数やその為のデータがある場所を最初に指定します。
#include <fd.h>
「fd.h」はFREEDOM jr.IIIを動かすための様々な関数が定義されています。モーションを再生する関数もここから読み込みます。
#include "share/Humanoid16Axis.h"
「Humanoid16Axis.h」にはバッテリ低下検出レベルとヒューマノイドロボット用のモーションデータの構造体が定義されています。
#include "share/HumanoidServoParam.c"
「HumanoidServoParam.c」にはモーションデータに対する各サーボモータのパラメータが定義されています。
#include "share/HumanoidSampleFlashData.c"
「HumanoidSampleFlashData.c」にはFLASHメモリに保存されるホームポジションとモーションデータが定義されています。
TApplyPart WholeBody
モーションを適用する部位を指定する構造体です。ここではFREEDOM jr.IIIの体全体を指定します。構造体の名前は任意です。
Priority: 1,
この適用部位のモーション再生優先度を指定します。優先度が高い(数値が小さい方が優先度が高い)部位に設定されたモーションを優先して再生します。
PartNum: 16,
この構造体において定義される軸数(サーボモータ数)を指定します。ここでは体全体ですので16軸となります。
part:{ &((THumanoid*)0)->Servos.RightLeg[0], // 右脚付根から &((THumanoid*)0)->Servos.RightLeg[1], &((THumanoid*)0)->Servos.RightLeg[2], &((THumanoid*)0)->Servos.RightLeg[3], &((THumanoid*)0)->Servos.RightLeg[4], // 右脚先まで &((THumanoid*)0)->Servos.LeftLeg[0], // 左脚付根から &((THumanoid*)0)->Servos.LeftLeg[1], &((THumanoid*)0)->Servos.LeftLeg[2], &((THumanoid*)0)->Servos.LeftLeg[3], &((THumanoid*)0)->Servos.LeftLeg[4], // 左脚先まで &((THumanoid*)0)->Servos.RightArm[0], // 右腕付根から &((THumanoid*)0)->Servos.RightArm[1], &((THumanoid*)0)->Servos.RightArm[2], // 右腕先まで &((THumanoid*)0)->Servos.LeftArm[0], // 左腕付根から &((THumanoid*)0)->Servos.LeftArm[1], &((THumanoid*)0)->Servos.LeftArm[2] // 左腕先まで }
モーションを適用する軸(サーボモータ)を指定します。ここでは体全軸を指定しています。
この値は実際にはモーションデータ内のアドレスです。
fd_Init (115200, false, _MONITOR_VOLTAGE);
FREEDOMjr.IIIを初期化する関数です。FREEDOMjr.IIIを動かすためには最初に必ず初期化します。fd_Initが関数の名前です。その後ろの括弧で囲まれカンマで区切られた3つの数字/文字が関数のパラメータ(引数)です。
1つめの引数「115200」は通信の速度(Baudrate)です。PCと直接接続する時にはPCとの通信速度、無線機器を使う場合はFREEDOMjr.IIIに搭載した無線機器との間の通信速度になります。前者の場合はSIMPLE TERMのBaudrateをこれに合わせる必要があります。後者の場合はFREEDOMjr.IIIに搭載した無線機器のBaudrateをこれに合わせる必要があります。
2つ目の引数「false」はリモートコントローラを使用するかどうかです。true(正)で無線コントローラを使う、false(誤)で使わないとなります。ここでは使わないのでfalseとします。
3つめの引数「_MONITOR_VOLTAGE」は電圧が何ボルトを下回ったらアラームを鳴らすかです。電池を使って動かしている場合、電池の残量が減ってくると電圧が下がってきます。_MONITOR_VOLTAGE以下になるとアラームが鳴りそれを知らせてくれます。Lipoバッテリは電池の残量が少なくなっているまま使用し続けると破損してしまいます。このアラームがなってから充電するのではなく、アラームが鳴らないようにこまめに電池の残量をチェックし、減ってきたら充電してください。詳しくはバッテリのマニュアルをご覧下さい。
fd_SetServoParameter (ConvToPPose (&HomePos), SVParam, fd_SVSize(SVParam));
サーボモータのパラメータを設定します。
1つ目の引数「ConvToPPose (&HomePos)」はFREEDOMjr.IIIのホームポジションです。ホームポジションは「HumanoidSampleFlashData.c」内で定義されています。
ConvToPPoseは型の変換をしています。変数には型があります。型はその変数がどういう変数かを表します。つまりHomePosがどういう変数かを変換しています。中味が変わってないのにどういう変数かを変えていいのかと思いますが、変換しても大丈夫なように変数を作っていますから大丈夫です。
試しにConvToPPoseを消してコンパイルしてみましょう。下のログに黄色いwarningがでます。型が違いますよと言っています。それでもコンパイルは成功します。これをFREEDOMjr.IIIに書き込んで動かすと普通に動きます。型は違うけど中味は一緒なので普通に動くというわけです。ただwarningが出るのが嫌なのでConvToPPoseをしています。
2つ目の引数「SVParam」はサーボモータのパラメータです。「HumanoidServoParam.c」中で定義されています。
3つめの引数「fd_SVSize(SVParam)」はSVParamのサイズです。fd_SVSizeはSVParamのサイズを計算する関数です。SVParamのサイズが分かっていればその値を直接書いても良いのですが、間違えると大変なことになるのでこのように書きます。
fd_SetMotion (ConvToPPose (GotoHome), 1, fd_MSize(GotoHome), 100, &WholeBody_1);
モーションの再生はこの1行だけできます。これはホームポジション(直立姿勢)になるモーションです。
1つ目の引数「ConvToPPose (WakeUp)」は直立姿勢になるモーションのデータです。
2つ目の引数「1」はWakeUpのモーションの1コマ目から再生することを表します。
3fd_MSize(GotoHome)
GotoHomeのサイズ分のコマまで再生することを表します。GotoHomeは1コマしかありませんのでfd_MSize(GotoHome)は1になります。1コマしかないのに間違えて2を入れると大変なことになりますのでfd_MSize(GotoHome)と書きます。
100
モーションを再生する速さです。速さと言っても200にすると速くなる訳ではありません。モーションデータの中でコマからコマへ移動する時間を設定するパラメータがあります。その時間の長さに対して%で指定します。100ならモーションデータの中で設定されている速さ(時間の長さ)で再生します。200に設定するとモーションデータの中で設定されている時間の倍の時間でモーションを再生します。つまり1/2の速さになります。逆に50にすると半分の時間(2倍の速さ)で再生します。
&WholeBody_1
FREEDOMjr.IIIのどの部位がモーションを再生するかを設定します。WholeBody_1は体全体です。その前についている&はポインタです。ポインタはアドレス(住所)です。データを関数にそのまま全部どさっと渡すよりは、「データはここのアドレスにあります」と言った方が計算が軽くなるので、関数にデータを渡す時はほとんどポインタになっています。