サーボ制御用の言語

ロボットに複雑な動きをさせるためには、サーボモーターを自在に制御しなければならない。膝なしロボット足郎1号にも、すでに16個のサーボモーターがついている。次の2号では、足だけなのにその倍、32個のサーボをつける予定だ。どう制御するのか。今は、C++のプログラムを書いて実行させている。引数を色々変えれば、ここのサーボ、あるいはサーボグループを変化させることはできるし、また、一連のプログラムの実行も可能だが、結局非常に面倒になっている。
そこで、こうした多数のサーボを制御するためのコンピュータ言語を作ろうと思う。例えばこんな感じである。
この言語の実際の応用例

##############################
# 足郎の制御のためのプログラム
# で始まる行はコメントになる
##############################
# 角度の定義、定義の名前、
#定義の内容を {  } 内に書き込む
# デリミタは基本、空白あるいはタブ
# 定義やコマンドの順序は関係ない
# ただし、 { }内記述の順番は意味を持つ
%defangles leftflac {
    #左の項の関節名は予約語
    LeftUpperLeft:12
    LeftUpperRight:12
    LeftLowerRight:12
    LeftLowerLeft:12
    RightUpperRight:12
    RightUpperLeft:12
    RightLowerRight:12
    RightLowerLeft:12
}
%defangles rightflac {
    RightUpperRight:12
    RightUpperLeft:12
    RightLowerLeft:12
    RightLowerRight:12
    LeftUpperLeft:12
    LeftUpperRight:12
    LeftLowerLeft:12
    LeftLowerRight:12
}
%defangles foreflac {
    LeftUpperFront:12
    LeftUpperBack:12
    LeftLowerFront:12
    LeftLowerBack:12
    RightUpperFront:12
    RightUpperBack:12
    RightLowerFront:12
    RightLowerBack:12
}
%defangles backflac {
    RightUpperFront:12
    RightUpperBack:12
    RightLowerFront:12
    RightLowerBack:12
    LeftUpperFront:12
    LeftUpperBack:12
    LeftLowerFront:12
    LeftLowerBack:12
}
#といった感じで、ここでは関節グループだが、
# 個別的なサーボの制御を書いても良い
# このような関節角度の定義を書き連ねる
# 実行の定義
# 使えるコマンドは今の所
# setAngle, speed, delay, stand, relax
%defexec walk1 {
    # この実行定義はループさせないで
    # exec は、再帰的呼び出しを可能にするか
    # 無限ループを避けなければならない
    # 次のものは繰り返させる
    # カンマの前後に空白を入れないこと
    exec:walk2,8
    # 実行が終了したら
    # 立たせる
    stand:all
}
%defexec walk2 {
    speed:10
    setAngle:leftflac
    # 100ミリ秒待機
    delay:100
}
# 実行させる
# exec 実行コマンド
# walk1 定義されている実行内容名
# 1回だけ実行する
%exec walk1 1
######## 以上、サンプル ###########

ファイルにして一気に実行もできて、また、一旦読み込んだら、コマンドプロンプトからインタープリターのようにも実行できるようにしたらいい。

逆振り子中の加速度

逆振り子中の加速度データが取れた。
加速度データは別スレッドで動かして取っているのだが、その間、なんだかサーボ制御の動きがおかしかった。RaspberryPiはコア4なのに、スレッド1個で動揺していては困るな。
青のX軸が前後の動きで、オレンジのYが左右、灰色のZ軸が上下である。
左右には割と安定して揺れているが、ぜん後には突然揺れが襲ってきている感じだ。
どういうタイミングで足を前後に出して行くかが問題だ。もう少し、一方の足への重心移動がはっきりと、安定して行われなければ、逆足の前への移動は難しいのかもしれない。