 |
 |
 |
 |
初めに 
歩くサンプルプログラムです。歩行はモーションとは違い、色々な計算を行って歩行を行っています。
ちょっと値を変えるだけで前進・後退・横歩きや腰の高さ、脚のあげる高さ、歩行周期を変えたりすることも可能です。
脚の長さや重心というパラメータもありますので、FREEDOMjr.IIIより大きいロボットを自作したとしてもこのプログラムを使うことが可能です。
歩行のためのパラメータ
TWalkParam WalkParam = {WalkParamが歩行のパラメータの構造体です。
LegDim :{ // 脚リンク長さ初期値 [mm *10]
R:{{{ 327, 74, 0}, // Pivot→ID1
{ 0, -80, -530}, // ID1→ID2
{ 0, 0, -680}, // ID2→ID3
{ 0, 0, -508}, // ID3→ID4
{ 0, 0, -145}, // ID4→ID5
{ 0, 0, -26}}}, // ID5→ID6
L:{{{ 327, -74, 0}, // Pivot→ID7
{ 0, 80, -530}, // ID77→ID8
{ 0, 0, -680}, // ID8→ID9
{ 0, 0, -508}, // ID9→ID10
{ 0, 0, -145}, // ID10→ID11
{ 0, 0, -26}}} // ID11→ID12
},
GravityPointHight : 2000, // 重心位置 [mm *10]
SideWidth : 400, // スタンス [mm *10]
Gravity : 98067, // 加速度 [mm/ss *10]
RLSide : LegR, // 踏み出す足脚の軸間の距離や重心などのFREEDOMjr.III固有のパラメータです。基本的にFREEDOMjr.IIIをそのまま使用している場合は変更する必要はありません。
ws : 0, // 歩幅 [mm] wh : 100, // 腰下げ量 [mm] lf : 90, // 脚持ち上げ量 [mm] pf : 0, // 脚押し込み量 [mm] kf : 0, // 脚持ち上げ保持量 [mm] rs : 0, // 右側横歩き歩幅 ls : 0, // 左側横歩き歩幅 ta : 0, // 旋回角度 [deg] speed : 10, // 歩行動作速度 decel : 50, // 減速停止時の減速量(decelの値だけws,ta,whが半周期毎に引かれる ls,rsはdecelの半分の値) cmd : CMD_STOP_WALK, // コマンド };
歩行を制御するためのパラメータです。初期値を設定しています。 wsを100にすると10mmの歩幅で前進し-100にすると10mmの歩幅で後退します。このパラメータを変更することでFREEDOMjr.IIIをコントロールします。
歩行を制御するためのキースキャン
void walkcmd (char c) {
if (WalkPart.MotionPlaying) {
switch (c) {
// Back and forth
case 'w':
WalkParam.ws += 50;
if (WalkParam.ws > 500) WalkParam.ws = 500;
else if (WalkParam.ws < -300) WalkParam.ws = -300;
break;
case 'x':
WalkParam.ws -= 50;
if (WalkParam.ws > 500) WalkParam.ws = 500;
else if (WalkParam.ws < -300) WalkParam.ws = -300;
break;
・
(中略)
・
}歩行を制御するためのキースキャンです。例えばwを1度入力するとWalkParamのwsを50増やす、つまり歩幅5mmで前進します。次にxを3回押すとwsを150減らす、つまりwsが-100になり歩幅10mmで後退します。
プラグイン設定
// 歩行サブルーチン登録 fd_AddPlugin ((TFDPluginFunc)Walk, 0, &WalkPart, &WalkParam);
歩行の関数はFREEDOMライブラリの中にはありません。ですので歩行関数をプラグインとして登録する必要があります。以下引数の紹介です。
(TFDPluginFunc)Walk
歩行の関数です。
0
歩行の関数は割り込みの中で実行されます。割り込みは一定周期で実行されるプログラムのようなものです。この引数を0にすると割り込みが実行されると必ずその中でWalk関数が実行されます。1にすると割り込み2回に1回Walk関数が実行されます。1回お休みします。
&WalkPart
歩行を行う体の部位を指定します。
TApplyPart WalkPart = {
Priority: 5,
PartNum: 10,
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],
}
};歩行を行うのは必ず脚なのでこのように設定します。
&WalkParam
歩行のためのパラメータの章で紹介したパラメータです。
歩行開始
WalkParam.cmd = CMD_START_WALK;
WalkParamのcmdをCMD_START_WALKにすると歩行を開始します。 あとはWalkParamのws(歩幅), wh(腰上げ量), lf(脚持ち上げ量), pf(脚押しこみ量), kf(脚持ち上げ保持量), rs(右側横歩き歩幅), ls(左側横歩き歩幅), speed(歩行動作速度)に好きな値を代入して歩行を操作します。ta(旋回角度)はFREEDOMjr.IIIには旋回軸が無いので機能しません。 cmdにCMD_DECEL_WALKを入れるdecel
