RaspberryPI 3でUSB音声出力の覚書

RaspberryPI3にアキバで買ってきたUSB DACと片側12.5Wのステレオアンプをつないで音を出してみた。
コマンドラインは、
$ aplay -D plughw:2,0 test.wav
でないとダメだった。DACとアンプをつないだ後、デバイスの指定がplughw:2,0でなければ、ならなかった。
音量調整は、
$ alsamixer -c 2
でする。家でやる限りは、10%程度でよかった。12.5Wのパワーはすごかった。大きなパワーにすると、少し音が割れ気味なのは、スピーカのせいか。

倒れるときの加速度センサーデータ、対応猶予時間

センサー(SPI通信)とサーボ制御(C2I通信)が別スレッドで、正常にできる様になった。
COSMで、サーボ制御とセンサー制御の両方のコマンドを新たに加えた。このコマンドで、ロボット歩行中のセンサーデータが取れる様になった。
ロボットが倒れる状況のデータが取れた。正確には、倒れるのを手で支える瞬間のデータが含まれているという意味だが。

こちらが、25秒間、10ミリ秒ごとにデータを取った図である。中央に、全ての軸が上下に大きく揺れているのが倒れるのを支えた瞬間のデータである。
その前後(500個分くらい)だけを表示させると以下の様になる。

こう見ると、倒れそうなことを一番特徴的に予見させるのがZ軸である。つまり、縦方向である。
倒れる状況になったのが、1132時点、つまり動き始めから11秒32の時点である。Z軸がおかしくなり始めたのが1107、つまり11秒07である。したがって、ほぼ250ミリ秒前に倒れそうになり始めたことがわかるのだ。1107の時点では、まだ予見できない。「注意」段階。はっきりと沈み込みがわかるのは、おそらく1116あたりだ。つまり、おかしくなり始めて、90ミリ秒で「倒れそうだ!!」という「警告」が出せそうである。倒れるまで、まだ、160ミリ秒もある。
倒れる160ミリ秒前に警告が出たら、なんとかなりそうなきがする。気象庁の地震や津波や台風の警報の様なものである。
ただ、この図では「どちらに倒れようとしているのか」が、わかりにくい。(1)X軸(+が前方)は、プラス側に偏っているので、前方に倒れそうだというのは、わかるかもしれない。(2)Y軸(+が右側)も、徐々にプラス側になっているので、右側に倒れそうだという予測がつくのかもしれない。(実際、その方に倒れたのだが、笑)
では、このとき何ができるのか?
(1)重心のある足を確認する。これは、本来、足の裏にでも圧力センサーをつけて、確認できる様にしたいが、現時点では安易すぎる解決だ。やはり、動かしているサーボモータの状況から理論的に予想できる「重心足」をチェックする。簡単だ。
(2)経験からだが、重心足の縦方向の姿勢を決定的に決めるのは、人間で言えば、足首の角度である。人間の場合は少し違うが、このロボットの場合は、重心足のくるぶし角度は決定的に重要だ。くるぶしの角度を規定しているのは4つのサーボモータである。これらのサーボ角度を微妙に転倒回避の方向に動かす。
(3)残りの全体もまた、影響する。なるべく、倒れる方向とは逆の方向に重心を持っていける様に調整する。どうするかはまだわからないが。
あと、スレッドが問題である。センサーを常時監視しているスレッドと、サーボモーターを制御しているスレッドが別々だ。センサーを監視しているスレッドが、直接サーボをいじると、トラブルになりそうだ。なるだろう。たとえ、mutexなどで、片方をロックしたりしてもダメだ。だから、サーボを制御しているスレッドが、センサーの警告を受け止める適切なメカニズムを作らなければならない。
センサースレッドが警告情報をどこかに書き込み、サーボスレッドがそれを細かく読んでチェックしているという状況だと、書き込みのバッティングも起こらない様な気がする。

二足歩行中の3軸加速度センサーのデータ

まだ、スレッド化はしていないのだが、センサーのspiによるシリアル通信がサーボを動かすi2cとバッティングしないかを見るために、ロボットが歩行中に、同じRaspberryPiから、センサープログラムも動かして影響を調べた。基本的に影響はない様だ。独立してデータを処理している。
歩行中のセンサーデータを初めてとったことになる。歩行の様子は、前の記事にあるゆっくりとしたものだ。データは20秒間の動きを10ミリ秒間隔で2000個とった。片側3歩くらいずつ動いている。
横方向(Y軸)は、揺れることが必要なので、まあいいが、縦の動き、前後の動き、X軸の動きの不安定さは、歩行の不安定さに直結しているが、これもまた、ある程度までは必要。
実際に速さを増やして、不安定歩行をさせてデータを取らなければならない。

3軸加速度センサーKXSD9-2050をRaspberryPIのSPIで動かす(2)

プログラムの方も形が整ったので、書いておこう。ロボットの頭頂部につけたセンサーをSPIで動かして捉えた結果の図は次のようになった。

Y軸方向(横方向)にロボットを揺らした。結果、X軸方向に細かい揺れも起こっているが、さすがに上下(Z軸方向)には動いていない。縦方向には、重力1(1g)がかかったままである。また、最初から、X軸方向に少し傾きがあることもわかる。

仕様書によると、データは、上記のSDOの様に入ってくる。基本3バイトの最初のバイトは、軸の指定。その後に続く2バイトは軸のデータの上位1バイトと下位1バイトの順に入ってくる。レジスタ二つ使っているので、3バイトのデータを送りながら2倍とずつずらすという作業が必要だ。以下のプログラムでその様にしているのがわかるでしょう。
したがって、C++のint値に変換するには、まず、intでとった上位ビットのデータを4ビットだけ左にシフトさせる。下位ビットは左側に4ビットしかないので、とったintデータを4ビットだけ右にシフトさせて、両者の論理和を取ると一つの整数データになることがわかる。以下のプログラムでそのような作業をしている。
この辺りの複雑さは、I2Cでやってもほぼ同じ。
以下にプログラムを貼り付ける。CとC++がごちゃまぜで、nanosleepなんかも使わなくても良いような気がするが、全部、動かしたまま貼り付けている。
※ 作成にあたって「Raspberry PiでSPI通信」を参照させていただいた。

#include <cstdlib>
#include <iostream>
#include <wiringPiSPI.h>
#include <wiringPi.h>
#include <stdio.h>
#include <time.h>
#include <errno.h>
using namespace std;
//定数定義
#define SS_PORT 8           //GPIO 8
#define SPI_CHANNEL 0       //SPIチャンネル
int main(void) {
    int speed; //通信速度(Hz)
    unsigned char spi_buff[3][3];    //送受信用バッファ
    struct timespec req;
    //sleep設定
    req.tv_sec = 0;
    req.tv_nsec = 130;    // 130ns
    speed = 1000000; //通信速度100kHz
    //バッファ初期化
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            spi_buff[i][j] = 0x0;
        }
    }
    cout << "SPIチャンネル初期化します" << endl;
    int spi_fd = 0;
    if ((spi_fd=wiringPiSPISetup(SPI_CHANNEL, speed)) < 0) {//SPIチャンネル初期化
        printf("wiringPiSPISetup error \n");
        return -1;
    }
    cout << "(使わない)spi_fd = " << spi_fd << endl;
    cout << "PIO初期化します" << endl;
    if (wiringPiSetupGpio() == -1) { //GPIO初期化
        printf("wiringPiSetupGpio error\n");
        return -1;
    }
    pinMode(SS_PORT, OUTPUT); //GPIO8を出力に設定
    digitalWrite(SS_PORT, 1); //SS信号初期化
    unsigned char com[2];
    com[0] = 0x0c;
    com[1] = 0xe3; // デフォルト設定を変更している
    digitalWrite(SS_PORT, 0);
    wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
    digitalWrite(SS_PORT, 1);
    com[0] = 0x0d;
    com[1] = 0x40; // デフォルト設定のまま
    nanosleep(&req, NULL);
    digitalWrite(SS_PORT, 0);
    wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
    digitalWrite(SS_PORT, 1);
    int iter = 0;
    int err;
    int xregH, xregL, xout;
    int yregH, yregL, yout;
    int zregH, zregL, zout;
    double xac, yac, zac;
    while (true) {
        //送信用データをバッファにセット
        //printf("******** iter %d: ********** \n", iter);
        for(int i=0;i<3;i++) {
            spi_buff[i][0] = 0x80+2*i;
            spi_buff[i][1] = 0;
            spi_buff[i][2] = 0;
            //printf("write <%d = 0x%x>\n", i, spi_buff[i][0]); //受信データを出力
            digitalWrite(SS_PORT, 0); //SS信号をLOW出力にして通信開始
            err = wiringPiSPIDataRW(SPI_CHANNEL, spi_buff[i], 3); //データ送受信
            if(err == -1){
                cout << "書き込みエラー(1) errno = " << errno << endl;
                break;
            }
            digitalWrite(SS_PORT, 1); //SS信号をHIGH出力にして通信終了
        }
            // デバイスからデータ取得
        xregH = spi_buff[0][1];
        xregL = spi_buff[0][2];
        xout = xregH << 4 | xregL >> 4;
        yregH = spi_buff[1][1];
        yregL = spi_buff[1][2];
        yout = yregH << 4 | yregL >> 4;
        zregH = spi_buff[2][1];
        zregL = spi_buff[2][2];
        zout = zregH << 4 | zregL >> 4;
        xac = (double) (xout - 2048) / (double) 819;
        yac = (double) (yout - 2048) / (double) 819;
        zac = (double) (zout - 2048) / (double) 819;
        cout << "No. : " << iter << "  X軸 : " << xac << "   Y軸 : " << yac << "   Z軸 : " << zac << endl;
        iter++;
        delay(10);
        if(iter == 500) break;
    }
    return 0;
}

3軸加速度センサーKXSD9-2050をRaspberryPIのSPIで動かす(1)

今日、ほぼ、丸1日、このことをやっていた。ほとんど進まなかったが、ここにきて光が差してきたので、これまでのことを記録しておく。
KXSD9-2050は、秋月電子のボードになったものだ。これは普通I2Cで使う。いや、これまで、使っていて、なかなかよく加速度を捉えていた。このブログの記事にもロボットの傾きを捉えているデータを何度も掲載してきた。なぜ今更、I2Cを諦めて、SPIでデータを取ろうとしているのか。理由は、I2Cは、サーボモータドライバのPCA9685でも動かしていて、それとどうにも相性が悪いようなのだ。使い方が悪いのかもしれないが。
特に、私としてはセンサーを別スレッドで常時動かして、ロボットの姿勢を監視していたい。揺れを最小限に抑えて、ロボットの早い歩行などに対応したいと思っていのだが、サーボとセンサを別スレッドで動かして、mutexでI2cに片側しかアクセスできないようにロックしても、サーボの角度設定にエラーが出る。センサーが使っている間、サーボを待たせることができないような感じだ。詳しくはわからない。
そこで、センサーの方をI2Cを使わないで、他のGPIOピンからアクセスしたいと思った。そうすれば、二つのスレッドがそれぞれの機器にアクセスするのを許すだろうと思っているからだ。そうならない可能性もあるが、やってみなきゃわからない。
それが理由なのだ。
朝から、それに挑戦したのだが、SPIは、面倒臭い。配線からして分かりにくい。データのやり取りも複雑だ。いや、結局複雑に考えすぎたのかもしれない。
ネット上にそもそも情報が少ない。KXSD9-2050は、SPIでもできるが基本I2Cでやるものだろう。海外サイトも含めてまとまった情報がなかった。だから、今回と次回では、私の様な1日無駄にしてしまう人が少ない様に、親切に記録しておこうと思う。
さて、途中の数え切れない失敗は、省略して、今、うまくいった、いい感じのところだけを記録しておく。なんだか、当たり前の様なことになっているのだが、この当たり前にたどり着くまでの彷徨は、無駄ではなかったと思いたい。
仕様書にある配線図及び秋月電子のマニュアル回路図は次のようなものである。どちらかだけをみても、さっぱりわからないのだ。両方同時に見ないと。

 
したがって、SPIの場合、(チャンネルゼロを使う)

KXSD9  Rspberry PI 3
SCLK (ピン5) SERIAL CLOCK (GPIO 11)
CS (ピン10) CS0 (GPIO 8)
SDO (ピン7) MISO (GPIO 9)
SDI (ピン9) MOSI (GPIO 10)
(ピン1とピン6) 3.3V電源
(ピン2) GND

となる。この辺りでも相当時間かかった!!(記録上はこれが一番大事。あとで、配線どうだったかなと、必ずこの記事を見ると思う)
配線の実際はこの様になっている。RaspberryPIの基盤の上に、専用の追加基盤がはめてある。黄色が2本使ってあって醜いが、線の色不足である。

続いて、プログラムである。配線にも自信がないときには、プログラムのせいでダメなのか、配線がダメなのか、そもそもボードが壊れているのか、あらゆることに疑いが出たが。なんとかなった。
プログラムについては、次回まとめて書く。

足郎1、歩行の改善

これまでの足郎1の歩行は、途中で大きな回転が入ったり不格好だったが、それを改善するために、これまで二足歩行の1サイクルを8フェーズに区切ってプログラムしていたのを6フェーズにして、途中で両足を下ろしたまま揃えるのをやめた。

動画に、歩行の改善が現れていると思う。
今回、超スローにしているが、少しずつ早い動きをつけていきたい。
サーボモータ制御のプログラミング言語名を、Cosmとさらに短くした。
Cosmなしに、ロボットのサーボ制御は、ほぼ不可能な状況だ。今回の動画の動きを作っているスクリプトをいかに示しておく。

##############################
# 2017年5月18日 Toyoaki WASHIDA
# 2017年5月20日 ver.2.2
# 2017年5月21日 ver.2.3
# 2017年5月23日 ver.2.4 ibot拡張子
# 2017年5月23日 ver.2.5
# サーボ制御言語 Cosm ver.1
# 足郎1号の制御のためのプログラム
# 「二足歩行」
# 左右の揺れとともに前への踏み出しが入っている
# ファイル名: ashiro1-5.cosm
##############################
# 角度パラメーター
%param init 0 # 左右に開いている初期状態の角度
%param openfront 50 # 足を前後に開く場合の基準角度
%param shiftbalance 30 # 片足に重心を移動するために傾けるメイン角度
%param support 14 # 開いた時に残った足の角度
%param liftup 6 # 重心が乗った足が、もう一方の足を釣り上げる角度
%param restflat 8 # 釣り上げられた足の角度(水平くらいがいい)
%param rhomb 22  # 菱形に重点移動角度
%param tiptoe 5 # 出した足のつま先
# スピード定義
%defspeed midium {
    interval:3
    steps:40
}
%defspeed slow {
    interval:5
    steps:30
}
# 右へ菱形に潰れる
%defangles rightrhomb {
    RightUpperRight:$rhomb
    RightUpperLeft:-$rhomb
    RightLowerRight:$rhomb
    RightLowerLeft:-$rhomb
    LeftUpperLeft:-$rhomb
    LeftUpperRight:$rhomb
    LeftLowerLeft:-$rhomb
    LeftLowerRight:$rhomb
}
%defangles leftrhomb {
    LeftUpperLeft:$rhomb
    LeftUpperRight:-$rhomb
    LeftLowerLeft:$rhomb
    LeftLowerRight:-$rhomb
    RightUpperRight:-$rhomb
    RightUpperLeft:$rhomb
    RightLowerRight:-$rhomb
    RightLowerLeft:$rhomb
}
# (状態)フェーズ0 直立
# (動作)フェーズ1の状態を作る
%defangles phase0 {
    RightUpperRight:-$liftup
    RightUpperLeft:$liftup
    RightLowerRight:$shiftbalance # 目一杯縮める
    RightLowerLeft:-$shiftbalance
    LeftUpperLeft:$shiftbalance-20 # 少し伸びている
    LeftUpperRight:-($shiftbalance-20)
    LeftLowerLeft:$restflat
    LeftLowerRight:-$restflat
    # 前後は不要か(初期の直立のままでいいから)
}
# フェーズ1
# (状態)右足に重点、左足が右足付近でつり上がっている
# (動作)左足を前側と左側にも開く 左に開くとは初期状態に戻すこと
%defangles phase1 {
    RightUpperRight:$init
    RightUpperLeft:$init
    RightLowerRight:$init
    RightLowerLeft:$init
    LeftUpperLeft:$init
    LeftUpperRight:$init
    LeftLowerLeft:$init
    LeftLowerRight:$init
    # 左足は目一杯開くが、右足は重心があるので、相当残す
    LeftUpperFront:-$openfront
    LeftUpperBack:$openfront
    # 左足のつま先を下げすぎないように-5する
    LeftLowerFront:-$tiptoe
    LeftLowerBack:$tiptoe
    RightUpperFront:$support
    RightUpperBack:-$support
    # ここに+5がないと、重心が後ろ過ぎて、後ろに倒れてしまう
    RightLowerFront:$support+5
    RightLowerBack:-($support+5)
}
# フェーズ2
# (状態)足が左右と前後に開かれている状態、左足が前に出ている
# (動作)股を閉じながら、左足に重心を移動する 右足を釣り上げる
%defangles phase2 {
    LeftUpperLeft:-$liftup
    LeftUpperRight:$liftup
    LeftLowerLeft:$shiftbalance # 目一杯縮める
    LeftLowerRight:-$shiftbalance
    RightUpperRight:$shiftbalance-20 # 少し伸びている
    RightUpperLeft:-($shiftbalance-20)
    RightLowerRight:$restflat
    RightLowerLeft:-$restflat
    # 前後はここで真っ直ぐに戻す
    LeftUpperFront:$init
    LeftUpperBack:$init
    LeftLowerFront:$init
    LeftLowerBack:$init
    RightUpperFront:$init
    RightUpperBack:$init
    RightLowerFront:$init
    RightLowerBack:$init
}
# フェーズ3
# (状態)左足に重心があり、右足がつり上がっている
# (動作)右足を右側に開きながら、前に踏み出す
%defangles phase3 {
    # 左右は揺れを直立に戻す、ことが開くことになるはず
    RightUpperRight:$init
    RightUpperLeft:$init
    RightLowerRight:$init
    RightLowerLeft:$init
    LeftUpperLeft:$init
    LeftUpperRight:$init
    LeftLowerLeft:$init
    LeftLowerRight:$init
    # 右足を前につりだす
    RightUpperFront:-$openfront
    RightUpperBack:$openfront
    # 左足のつま先を下げすぎないように-5する
    RightLowerFront:-$tiptoe
    RightLowerBack:$tiptoe
    LeftUpperFront:$support
    LeftUpperBack:-$support
    LeftLowerFront:$support+5
    LeftLowerBack:-($support+5)
}
# フェーズ4
# (状態)右足が前に出た状態で、足が開いている
# (動作)左足を右足に近づけながら、重心移動、左足を釣り上げる
%defangles phase4 {
    RightUpperRight:-$liftup
    RightUpperLeft:$liftup
    RightLowerRight:$shiftbalance # 目一杯縮める
    RightLowerLeft:-$shiftbalance
    LeftUpperLeft:$shiftbalance-20 # 少し伸びている
    LeftUpperRight:-($shiftbalance-20)
    LeftLowerLeft:$restflat
    LeftLowerRight:-$restflat
    # 前後はここで真っ直ぐに戻す
    LeftUpperFront:$init
    LeftUpperBack:$init
    LeftLowerFront:$init
    LeftLowerBack:$init
    RightUpperFront:$init
    RightUpperBack:$init
    RightLowerFront:$init
    RightLowerBack:$init
}
# 実行の定義
# メインルーチンの定義
%defexec walk1 {
    # まず、立たせる
    stand:all
    # 1000ms待機
    delay:1000
    # 右に揺れ始める
    speed:slow
    setAngle:rightrhomb
    setAngle:phase0
    # 歩行のサブルーチンに移動する
    exec:walk2,3
    # 実行が終了したら
    # 緩和状態で立つ
    delay:1000
    relax:all
}
# サブルーチンの定義
%defexec walk2 {
    # 指定されたスピードは再指定がない限り有効
    speed:midium
    # 右足に重心を置いた状態から、左足を前に出す
    setAngle:phase1
    # 左に重点を置いて、右足を釣り上げる
    setAngle:leftrhomb
    setAngle:phase2
    # 右足を前後に開く
    setAngle:phase3
    # 右足に重点を写し、左足を釣り上げる
    setAngle:rightrhomb
    setAngle:phase4
    # 100ミリ秒待機
    # delay:100
}
# exec 実行コマンド
# walk1 定義されている実行内容名
# 1回だけ実行する
%exec walk1 1
######## 以上 ###########

サーボ制御言語COSMの改訂

COSMの文法を少し改定した。
(1)最も大きな改訂は、値にパラメータを利用した数式が使えるようになったこと。数式は全て倍精度(double)で演算され、必要に応じて整数(int)に型変換される。
ex.   10*($spstp + 7)/ $mul
それに伴って、これまで、サーボ角度のパラメータを、ふにする場合、新たにパタメータを定義しなければならなかったが、不要になり、使用時にパラメータの前に '-' (マイナス記号)をつけるだけで良くなった。(構文解析で単項マイナスの処理が面倒なので、冒頭のマイナスの前に0をつけているだけなのだが(笑) -$test の時 0-$testする)
(2)角度 defangles、スピード defspeed、実行 defexec の定義は、キーと値を':'で挟んだものになっているが、値が数式で評価されるために、その中に空白を入れたい場合もある(入れなくてもいいのだが)。その場合は、あたいの全体を [  ] で囲む必要がある。その場合でも':'と [  ] の間に空白を挟んではいけない。
(3)logコマンドの導入。値をログとして出力する(現時点ではコンソールだが、いずれlogファイルへの出力にする)。値のうち  " で挟まれている部分は文字列として出力されるが、それ以外の部分は式として評価され、評価され値が出力される。
今後、まず、変数(%variable)を導入したい。変数は、@xyzのように、@マークで区別するかな。
さらに、この変数が重要な役割を果たす、センサーチェックコマンドを導入する。
センサーのある方向の値がどうなったばいに、角度をどう変化されるとかして、色々な事態に対応できるようにするわけだ。そうなると、比較演算子も必要になるな。
今回の改訂のテストを行ったスクリプトファイルの内容を以下に掲載しておく。

#
# Commandの式評価のテストCosm
# 2017年5月21日
#
 # コメントのテスト0
# パラメータは名前と値で、名前の冒頭は$で始まってはならない、他の文字は大丈夫
# 同じ名前のパラメータの定義があったら、後で定義された方が有効になる
%param test1 8 # コメントのテスト1
%param test2 5
%param inter 9
%param spint 8
%param spstp 5
%param mul 3
%defspeed slow {  # コメントのテスト2
    # キーと値のついになっている、':'の前後に空白があってはならない
    interval:$mul*($spint+2)  # コメントのテスト3
    # 値に空白を入れたい場合は、 [ ]で囲む
    # ただし、':'と[ ]の間に空白があってはならない
    # 値は、いかなる場合も、パラメータを交えた数式として評価される
    # speed の値はどちらも負であってはならない
    # さらにstepsはゼロでもいけない
    steps:[10*($spstp + 7)/ $mul]  # コメントのテスト4
}
%defangles angles1 {
    # 値に空白を入れたい場合は、全体を [ ] で囲むこと
    # ':' の前後に空白は入れない
    lefttopangle:[0.5*($test1 + $test2)]  # コメントのテスト5
}
%defexec exec1 {
    # 次のlogはデバグ済み: " "挟まれていない項は数式として評価される
    # 空白が値にある場合は、全体が [ ] で囲まれている必要がある
    log:["角度 [ "  0.5*($test1 + $test2) " ] を セットします"] # コメントのテスト6
    # 角度もデバグ済み
    setAngle:angles1  # コメントのテスト7
    # delayの値は式として評価される、[ ]の件などは同じ
    delay:$inter*0.6  # コメントのテスト8
    speed:slow  # コメントのテスト9
}
%exec exec1 1 #  # コメントのテスト10

足郎1を二足歩行させてみる

次の段階として、前への踏み出しをするようにして、二足歩行させてみた。

原理としては、二足歩行であるが、あまりに不恰好だ。膝がないために、足の折りたたみができないので、重心を片足に乗せて、重心のない方の足をしっかり釣り上げなければならず、負担が大きい(下記、サーボ角度図参照)。
二足歩行のために、足の裏に厚さ3ミリの天然ゴムを貼ったことを記しておこう。不要な回転を止めるためだが、役に立った。ただ、それでも回転は完全に止めきれていないが。これも、膝がないことが影響しているか。
サーボ16個フルに動かしているので、スピード感がなくなってきている。しかし、まだ、スピードを出す余裕はある。
サーボ角度図の最新版は次のようなものである。

動かすためのプログラムを以下に示しておく。プログラミングで、speedの設定を、stepsとintervalで定義するように文法を改訂している。次の文法改訂では、パラメータの演算、式の評価を入れて、さらに、センサーの制御も入れて、プログラミング言語らしくしたい。(Cosm: Controler Of ServoMOtor)

##############################
# 2017年5月18日 Toyoaki WASHIDA
# 2017年5月20日 ver.2.2
# 足郎1号の制御のためのプログラム
# 「二足歩行」
# 左右の揺れとともに前への踏み出しが入っている
# ファイル名: ashiro1-1.ibot
# '#'で始まる行はコメントになる
##############################
# 角度の定義、定義の名前、
# 定義の場合は、内容を {  } 内に書き込む
# デリミタは基本、空白
# %で始まる定義やコマンドの順序は関係ない
# ただし、 { }内記述の順番は意味を持つ(順番の通り実行される)
# パラメーターは、サーボ角度の値(足の角度ではない)
# (注)現状で、上のサーボと下のサーボで方向が逆になっている
# スピード、delayの値で使うことが可能
# パラメーター使用時には'$'の接頭文字が必要
# 角度パラメーター
%param angle0 0
%param angle1 8
%param angle2 14
#%param angle3 16
%param angle3 22
%param angle4 6
%param -angle1 -8
%param -angle2 -14
#%param -angle3 -16
%param -angle3 -22
%param -angle4 -6
# 前へ踏み出す
%param angle5 20
%param -angle5 -20
# 前に出した足のつま先がつっかからないようにする
%param -angle5S -5
# 踏み出しを半分戻す
%param angle6 10
%param -angle6 -10
# スピード定義
# intervalは角度設定の合間に置くms
# stepsは、目的角度まで幾つ区切って進むか
# speedは両者とも定義されていなければエラーになる
# デフォルトはmidium
%defspeed fast {
    interval:2
    steps:10
}
%defspeed midium {
    interval:6
    steps:10
}
%defspeed slow {
    interval:9
    steps:10
}
# サーボ角度の定義集
# 直立状態
%defangles zerophaseSide {
    # 左の項のサーボ名はシステム予約語
    #
    # 左右の動きに関わるサーボ
    LeftUpperLeft:$angle0
    LeftUpperRight:$angle0
    LeftLowerRight:$angle0
    LeftLowerLeft:$angle0
    RightUpperRight:$angle0
    RightUpperLeft:$angle0
    RightLowerRight:$angle0
    RightLowerLeft:$angle0
    #
    # 前後の動きに関わるサーボ
    LeftUpperFront:$angle0
    LeftUpperBack:$angle0
    LeftLowerFront:$angle0
    LeftLowerBack:$angle0
    RightUpperFront:$angle0
    RightUpperBack:$angle0
    RightLowerFront:$angle0
    RightLowerBack:$angle0
}
# 最初の一歩の前段階
# 右への傾きの第1段階、菱形に潰れる
%defangles rightphase1-pre {
    RightUpperRight:$angle2
    RightUpperLeft:$-angle2
    RightLowerRight:$angle2
    RightLowerLeft:$-angle2
    LeftUpperLeft:$-angle2
    LeftUpperRight:$angle2
    LeftLowerLeft:$-angle2
    LeftLowerRight:$angle2
    # 前後の動きに関わるサーボ
    # 不要と思われる
}
# 歩行中の
# 右への傾きの第1段階、菱形に潰れる
%defangles rightphase1 {
    RightUpperRight:$angle2
    RightUpperLeft:$-angle2
    RightLowerRight:$angle2
    RightLowerLeft:$-angle2
    LeftUpperLeft:$-angle2
    LeftUpperRight:$angle2
    LeftLowerLeft:$-angle2
    LeftLowerRight:$angle2
    # 前後は半分くらい戻す
    LeftUpperFront:$angle6
    LeftUpperBack:$-angle6
    LeftLowerFront:$angle6
    LeftLowerBack:$-angle6
    RightUpperFront:$-angle6
    RightUpperBack:$angle6
    RightLowerFront:$-angle6
    RightLowerBack:$angle6
}
# 右への傾きの第2段階、
# 右足に重心を十分移動し、左足をできる限りあげる
%param angle3U 10
%param -angle3U -10
%defangles rightphase2 {
    RightUpperRight:$-angle4
    RightUpperLeft:$angle4
    RightLowerRight:$angle3
    RightLowerLeft:$-angle3
    LeftUpperLeft:$angle3U
    LeftUpperRight:$-angle3U
    LeftLowerLeft:$angle1
    LeftLowerRight:$-angle1
    # 前後はここで真っ直ぐに戻す
    LeftUpperFront:$angle0
    LeftUpperBack:$angle0
    LeftLowerFront:$angle0
    LeftLowerBack:$angle0
    RightUpperFront:$angle0
    RightUpperBack:$angle0
    RightLowerFront:$angle0
    RightLowerBack:$angle0
}
# 右への傾きの第3段階、
# 左足を前に踏み出す
%defangles rightphase3 {
    # 左右は揺れを直立に戻す
    LeftUpperLeft:$angle0
    LeftUpperRight:$angle0
    LeftLowerRight:$angle0
    LeftLowerLeft:$angle0
    RightUpperRight:$angle0
    RightUpperLeft:$angle0
    RightLowerRight:$angle0
    RightLowerLeft:$angle0
    # (left)前に踏み出す 後ろ足(right)の送り
    LeftUpperFront:$-angle5
    LeftUpperBack:$angle5
    LeftLowerFront:$-angle5S
    LeftLowerBack:$angle5
    RightUpperFront:$angle5
    RightUpperBack:$-angle5
    RightLowerFront:$angle5
    RightLowerBack:$-angle5
}
# (改訂S:右足をあまり開かない)左への傾きの第1段階、菱形に潰れる
# angle2 は本来14
# パラメータはどこにおいても良いので
%param angle2S 4
%param -angle2S -4
%defangles leftphase1 {
    LeftUpperLeft:$angle2
    LeftUpperRight:$-angle2
    LeftLowerLeft:$angle2
    LeftLowerRight:$-angle2
    # ここを変形
    RightUpperRight:$-angle2S
    RightUpperLeft:$angle2S
    # ここまで
    RightLowerRight:$-angle2
    RightLowerLeft:$angle2
    # 前後は半分くらい戻す
    LeftUpperFront:$-angle6
    LeftUpperBack:$angle6
    LeftLowerFront:$-angle6
    LeftLowerBack:$angle6
    RightUpperFront:$angle6
    RightUpperBack:$-angle6
    RightLowerFront:$angle6
    RightLowerBack:$-angle6
}
# (改訂S:)左への傾きの第2段階、
# 左足に重心を十分移動し、右足をできる限りあげる
# LeftLowerLeftをもっと引く
%param angle3S 32
%param -angle3S -32
%param angle4S 0
%param -angle4S 0
%param angle3T 10
%param -angle3T -10
%defangles leftphase2 {
    LeftUpperLeft:$-angle4S
    LeftUpperRight:$angle4S
    # ここを変化させた
    LeftLowerLeft:$angle3S
    LeftLowerRight:$-angle3S
    # ここまで
    RightUpperRight:$angle3T
    RightUpperLeft:$-angle3T
    RightLowerRight:$angle1
    RightLowerLeft:$-angle1
    # 前後はここで真っ直ぐに戻す
    LeftUpperFront:$angle0
    LeftUpperBack:$angle0
    LeftLowerFront:$angle0
    LeftLowerBack:$angle0
    RightUpperFront:$angle0
    RightUpperBack:$angle0
    RightLowerFront:$angle0
    RightLowerBack:$angle0
}
# 左への傾きの第3段階、
# 右足を前に踏み出す
%defangles leftphase3 {
    # 左右は揺れを直立に戻す
    LeftUpperLeft:$angle0
    LeftUpperRight:$angle0
    LeftLowerRight:$angle0
    LeftLowerLeft:$angle0
    RightUpperRight:$angle0
    RightUpperLeft:$angle0
    RightLowerRight:$angle0
    RightLowerLeft:$angle0
    # (right)前に踏み出す 後ろ足(left)の送り
    LeftUpperFront:$angle5
    LeftUpperBack:$-angle5
    LeftLowerFront:$angle5
    LeftLowerBack:$-angle5
    RightUpperFront:$-angle5
    RightUpperBack:$angle5
    RightLowerFront:$-angle5S
    RightLowerBack:$angle5
}
# 実行の定義
# 使えるコマンドは今の所
# setAngle, speed, delay, stand, relax
# メインルーチンの定義
%defexec walk1 {
    # この実行定義はループさせないで
    # exec は、再帰的呼び出しを可能にするか
    # 無限ループを避けなければならない
    # 次のものは繰り返させる
    # カンマの前後に空白を入れないこと
    #
    # まず、立たせる
    stand:all
    # 1000ms待機
    delay:1000
    # 最初の歩き出しは、前後を開いていないため、
    # メインルーチンに入れておく
    # 右に揺れ始める
    speed:midium
    setAngle:rightphase1-pre
    # 歩行のサブルーチンに移動する
    exec:walk2,3
    # 実行が終了したら
    # 立たせる
    stand:all
}
# サブルーチンの定義
%defexec walk2 {
    # 指定されたスピードは再指定がない限り有効
    # 足を上げる
    speed:midium
    setAngle:rightphase2
    setAngle:rightphase3
    # 左に揺れ始める
    speed:midium
    setAngle:zerophaseSide
    setAngle:leftphase1
    # 足を上げる
    speed:midium
    setAngle:leftphase2
    setAngle:leftphase3
    # 右に揺れ始める
    speed:midium
    setAngle:zerophaseSide
    setAngle:rightphase1
    # 100ミリ秒待機
    # delay:100
}
# 実行させる
# exec 実行コマンド
# walk1 定義されている実行内容名
# 1回だけ実行する
%exec walk1 1
######## 以上 ###########

足郎1に足踏みさせる

歩行にはどうしても一時的にでも片足立ちになることが必要なので、足郎1にもそれをさせてみた。サーボ制御言語の応用の意味もあった。

まだ、安定していないが、微調整をしていきたい。左右の動きに関する8個のサーボの角度マップは以下のようである。前後の8個は動かしていない。左右の動きの一部強い非対称性は、重心が微妙に右側にあることと、サーボの引き方のグダグダ感によるものだと思う。(この角度に向かって現在の角度から、ステップを刻みながら近づいていくように、次のスクリプト言語でプログラムされている)

以下に、上記のサーボの制御言語で書かれたスクリプトを記しておく。動画はこのスクリプトで動かしたものである。見れば、言語の文法はおよそわかっていただけると思う。最初は定義が羅列されていて、最後の方に実行コマンドが入っている

##############################
# 2017年5月18日 Toyoaki WASHIDA
# 2017年5月20日 ver.2
# 足郎1号の制御のためのプログラム
# 「足踏み」
# ファイル名: ashiro1-1.ibot
# '#'で始まる行はコメントになる
##############################
# 角度の定義、定義の名前、
# 定義の場合は、内容を {  } 内に書き込む
# デリミタは基本、空白
# %で始まる定義やコマンドの順序は関係ない
# ただし、 { }内記述の順番は意味を持つ(順番の通り実行される)
# パラメーターは、サーボ角度の値(足の角度ではない)
# (注)現状で、上のサーボとしたのサーボで方向が逆になっている
# スピード、delayの値で使うことが可能
# パラメーター使用時には'$'の接頭文字が必要
# 角度パラメーター
%param angle0 0
%param angle1 8
%param angle2 14
%param angle3 16
%param angle4 6
%param -angle1 -8
%param -angle2 -14
%param -angle3 -16
%param -angle4 -6
# スピードパラメーター
# ステップが刻まれる回数なので
# 正の整数、数字が大きくなるほど遅い
# 1ステップの最後に10msの待機を入れている
%param fast 10
%param slow 10
# 直立状態
%defangles zerophaseSide {
    #左の項のサーボ名はシステム予約語
    LeftUpperLeft:$angle0
    LeftUpperRight:$angle0
    LeftLowerRight:$angle0
    LeftLowerLeft:$angle0
    RightUpperRight:$angle0
    RightUpperLeft:$angle0
    RightLowerRight:$angle0
    RightLowerLeft:$angle0
}
# 右への傾きの第1段階、菱形に潰れる
%defangles rightphase1 {
    RightUpperRight:$angle2
    RightUpperLeft:$-angle2
    RightLowerRight:$angle2
    RightLowerLeft:$-angle2
    LeftUpperLeft:$-angle2
    LeftUpperRight:$angle2
    LeftLowerLeft:$-angle2
    LeftLowerRight:$angle2
}
# 右への傾きの第2段階、
# 右足に重心を十分移動し、左足をできる限りあげる
%defangles rightphase2 {
    RightUpperRight:$-angle4
    RightUpperLeft:$angle4
    RightLowerRight:$angle3
    RightLowerLeft:$-angle3
    LeftUpperLeft:$angle3
    LeftUpperRight:$-angle3
    LeftLowerLeft:$angle1
    LeftLowerRight:$-angle1
}
# 左への傾きの第1段階、菱形に潰れる(使わなかった、以下の改訂版を使った)
%defangles leftphase1 {
    LeftUpperLeft:$angle2
    LeftUpperRight:$-angle2
    LeftLowerLeft:$angle2
    LeftLowerRight:$-angle2
    RightUpperRight:$-angle2
    RightUpperLeft:$angle2
    RightLowerRight:$-angle2
    RightLowerLeft:$angle2
}
# (改訂S:右足をあまり開かない)左への傾きの第1段階、菱形に潰れる
# angle2 は本来14
%param angle2S 4
%param -angle2S -4
%defangles leftphase1S {
    LeftUpperLeft:$angle2
    LeftUpperRight:$-angle2
    LeftLowerLeft:$angle2
    LeftLowerRight:$-angle2
    # ここを変形
    RightUpperRight:$-angle2S
    RightUpperLeft:$angle2S
    # ここまで
    RightLowerRight:$-angle2
    RightLowerLeft:$angle2
}
# 左への傾きの第2段階、これは使わずに、次の改訂版を使った
# 左足に重心を十分移動し、右足をできる限りあげる
%defangles leftphase2 {
    LeftUpperLeft:$-angle4
    LeftUpperRight:$angle4
    LeftLowerLeft:$angle3
    LeftLowerRight:$-angle3
    RightUpperRight:$angle3
    RightUpperLeft:$-angle3
    RightLowerRight:$angle1
    RightLowerLeft:$-angle1
}
# (改訂S:)左への傾きの第2段階、
# 左足に重心を十分移動し、右足をできる限りあげる
# LeftLowerLeftをもっと引く
# この定義に関係するだけのパラメータを書いておく
%param angle3S 32
%param -angle3S -32
%param angle4S 0
%param -angle4S 0
%param angle3T 10
%param -angle3T -10
%defangles leftphase2S {
    LeftUpperLeft:$-angle4S
    LeftUpperRight:$angle4S
    # ここを変化させた
    LeftLowerLeft:$angle3S
    LeftLowerRight:$-angle3S
    # ここまで
    RightUpperRight:$angle3T
    RightUpperLeft:$-angle3T
    RightLowerRight:$angle1
    RightLowerLeft:$-angle1
}
# 前後の動き、使っていない
%defangles foreflac {
    LeftUpperFront:$angle
    LeftUpperBack:$angle
    LeftLowerFront:$angle
    LeftLowerBack:$angle
    RightUpperFront:$angle
    RightUpperBack:$angle
    RightLowerFront:$angle
    RightLowerBack:$angle
}
# 前後の動き、使っていない
%defangles backflac {
    RightUpperFront:$angle
    RightUpperBack:$angle
    RightLowerFront:$angle
    RightLowerBack:$angle
    LeftUpperFront:$angle
    LeftUpperBack:$angle
    LeftLowerFront:$angle
    LeftLowerBack:$angle
}
# 実行の定義
# 使えるコマンドは今の所
# setAngle, speed, delay, stand, relax
# メインルーチンの定義
%defexecs walk1 {
    # この実行定義はループさせないで
    # exec は、再帰的呼び出しを可能にするか
    # 無限ループを避けなければならない
    # 次のものは繰り返させる
    # カンマの前後に空白を入れないこと
    exec:walk2,3
    # 実行が終了したら
    # 立たせる
    stand:all
}
# サブルーチンの定義
%defexecs walk2 {
    # 指定されたスピードは再指定がない限り有効
    # 右に揺れ始める
    speed:$fast
    setAngle:zerophaseSide
    setAngle:rightphase1
    # 足を上げる
    speed:$slow
    setAngle:rightphase2
    setAngle:rightphase1
    # 左に揺れ始める
    speed:$fast
    setAngle:zerophaseSide
    setAngle:leftphase1S
    # 足を上げる
    speed:$slow
    setAngle:leftphase2S
    setAngle:leftphase1
    # 100ミリ秒待機
    # delay:100
}
# 実行させる
# exec 実行コマンド
# walk1 定義されている実行内容名
# メインルーチンは1回だけ実行する
%exec walk1 1
######## 以上 ###########