Q&A
この提示されたコードはどういう動作をするんでしょうか
前提・実現したいこと
二足歩行ロボットの歩行のプログラムを作っています。
しかし、歩行関数の部分の計算がまったく意味が分かりません。
歩行関数から座標が求まることはなんとなくわかるのですが、if文もどういう意味か分からず、その下の計算も全く理解できません。これはどういうのでという感じで、詳しく教えてください。
また、この歩行関数を用いて横歩行を行いたいのですが、その場合逆運動学が使えない(※1)ので、どうやってプログラムを組めばいいのかわかりません。そこについても教えてください。
※1横歩行はロール軸、前後歩行はピッチ軸のため横歩行で使える角度が一つしかない。
（歩行プログラムでwhile文でdelayを使って行うと、変な動きをする。例）ボタンを離しても動くという誤動作が起こる。）
###プログラムの説明
このプログラムは、歩行プログラムに歩行関数(TLLD)から得られた座標を逆運動学(LF)の計算式に代入して、座標からサーボモータを動かすための角度を求めて、サーボモータを動かして歩行を実現させています。
該当のソースコード
/////////////////歩行関数(左足のx軸)////////////////////
void TLLD(float tL, float v_b) {
//前後方向 left（左）
if (tL < eps_X * T / 2.0) {
v_xL = -v_b;
xL = -v_b * tL;
}
else if (tL < (1.0 - eps_X / 2.0)*T) {
v_xL = (v_b / (1.0 - eps_X)) * (1.0 - cos(2 * pi / ((1.0 - eps_X) * T) * (tL - T * (eps_X / 2.0)))) - v_b;
xL = (v_b / (1.0 - eps_X)) * (tL - (eps_X * T / 2.0) - (T * (1.0 - eps_X) / (2.0 * pi)) * sin(2 * pi / ((1.0 - eps_X) * T) * (tL - T * (eps_X / 2.0)))) - (v_b * tL);
}
else {
v_xL = -v_b;
xL = -v_b * (tL - T);
}
}
///////////////////////////////////////////////
/////////////////歩行関数（左足のz軸）////////////////////
void TLH(float tL, float h) {
//足上げ高さ
if (tL < eps_Z * T / 2.0) {
v_zL = 0.0;
zL = -H;
}
else if (tL < (1.0 - eps_Z / 2.0)*T) {
v_zL = pi * h / ((1.0 - eps_Z) * T) * sin(2.0 * pi / ((1.0 - eps_Z) * T) * (tL - eps_Z / 2.0 * T));
zL = -h / 2.0 * cos(2.0 * pi / ((1.0 - eps_Z) * T) * (tL - eps_Z / 2.0 * T)) + h / 2.0 - H;
}
else {
v_zL = 0.0;
zL = -H;
}
}
///////////////////////////////////////////////
/////////////////逆運動学//////////////////////
void LF(float x, float y, float z) {
uint16_t val4, val5, val6;
/逆運動学*/
L3 = sqrt((x * x) + (z * z)); //原点からの目標座標
K = atan2(z, x); //座標(x,z)を指定して角度を求める
A = degrees( K - acos(((L1 * L1) + (L3 * L3) - (L2 * L2)) / (2 * L1 * L3)) ); //角度1
AA = (90.0 - A);
B = degrees( PI - acos(((L1 * L1) + (L2 * L2) - (L3 * L3)) / (2 * L1 * L2)) ); //角度2
C = -(A + B) + 90.0;
val4 = map(AA, -135, 135, 3500, 11500); //サーボモータは3500~11500の間で動く
val5 = map(B, -135, 135, 3500, 11500);
val6 = map(C, -135, 135, 3500, 11500);
/ピッチ軸/
servoset(12, val6 + 400); //ID_12番をval3の角度に +で↓ 左足首（手動で治す）
servoset(10, val5 - 80); //ID_10番をval2の角度に -で後ろ （手動で治す）
servoset(8, val4 - 120); //ID_8番をval1の角度に +で後ろ （手動で治す）
}
////////////////////////////////////////////
////////////////歩行プログラム////////////////////
uint16_t val7,val8;
float dt = 0.04,
v_b = 20.0,
h = -25.0;
if(t > T){
t -= T;
}
if(tR > T){
tR -= T;
}
if(tL > T){
tL -= T;
}
/* 右足の進行方向平面の動き /
TRLD(tR,v_b); //歩行関数(TRLD)でx座標を求める
TRH(tR,h); //歩行関数(RF)でz座標を求める
RF(xR, yR, zR); //逆運動学よりサーボモータを動かす
/ 左足の進行方向平面の動き /
TLLD(tL,v_b); //歩行関数(TLLD)でx座標を求める
TLH(tL,h); //歩行関数(LF)でz座標を求める
LF(xL, yL, zL); //逆運動学よりサーボモータを動かす
servoset(15, 6500); //腕
servoset(13, 9000); //腕
/ 時間を進める */
tR += dt;
tL += dt;
}
////////////////////////////////////////////
補足情報（初期設定）
/////////////////////初期設定/////////////////////////
v_b=20.0 //歩行速度[mm/s]
eps_x=0.7 //水平方向デューティ比
tL //時間[s]?
v_xL //左の水平方向速度[mm/s]
xL //左の水平方向位置[mm]
dt=0.04 //サンプリングタイム
T=2.0 //歩行周期[s]
tL=3.0*T/4.0;
pi=3.1415926535
/////////////////////////////////////////////////////
こういうサイトで臆面もなく「至急」などという言葉を使わないでください。答える側はそんなの知りません。
「ボタンを離しても動くという誤動作が起こる」と捉えるのではなく、「プログラムに誤りがあるため、ボタンを離しても動くという動作が起こる」と考えないとデバッグなんて出来ないですよ。
逆運動学の関数 LF の14行目 "servoset(12, val6 + 400); //ID_12番をval3の角度に +で↓ 左足首（手動で治す）" のように、コードでは val6が使われていて、コメントでは val3を使うように書かれている部分はコードのコピーミスなどの可能性が高いので、信じ込まないほうが良いです。
あなたの回答
tips
プレビュー
