カテゴリー: ロボット作り

少し早足で歩かせて、追加で「倒れるデータ」を取った。

３度も倒れそうになった。基本的に、前に確認したものと同じであり、倒れそうな方向にX軸とY軸が少しずつ偏り、数百ミリ秒て前から、Z軸の沈み込みが始まる。

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

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

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

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

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

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

仕様書によると、データは、上記のSDOの様に入ってくる。基本３バイトの最初のバイトは、軸の指定。その後に続く２バイトは軸のデータの上位１バイトと下位１バイトの順に入ってくる。レジスタ二つ使っているので、３バイトのデータを送りながら２倍とずつずらすという作業が必要だ。以下のプログラムでその様にしているのがわかるでしょう。
したがって、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;
}

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

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

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

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

これまでの足郎１の歩行は、途中で大きな回転が入ったり不格好だったが、それを改善するために、これまで二足歩行の１サイクルを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
}
# （状態）フェーズ０ 直立
# （動作）フェーズ１の状態を作る
%defangles phase0 {
    RightUpperRight:-$liftup
    RightUpperLeft:$liftup
    RightLowerRight:$shiftbalance # 目一杯縮める
    RightLowerLeft:-$shiftbalance
    LeftUpperLeft:$shiftbalance-20 # 少し伸びている
    LeftUpperRight:-($shiftbalance-20)
    LeftLowerLeft:$restflat
    LeftLowerRight:-$restflat
    # 前後は不要か（初期の直立のままでいいから）
}
# フェーズ１
# （状態）右足に重点、左足が右足付近でつり上がっている
# （動作）左足を前側と左側にも開く 左に開くとは初期状態に戻すこと
%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)
}
# フェーズ２
# （状態）足が左右と前後に開かれている状態、左足が前に出ている
# （動作）股を閉じながら、左足に重心を移動する 右足を釣り上げる
%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
}
# フェーズ３
# （状態）左足に重心があり、右足がつり上がっている
# （動作）右足を右側に開きながら、前に踏み出す
%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)
}
# フェーズ４
# （状態）右足が前に出た状態で、足が開いている
# （動作）左足を右足に近づけながら、重心移動、左足を釣り上げる
%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の文法を少し改定した。
（１）最も大きな改訂は、値にパラメータを利用した数式が使えるようになったこと。数式は全て倍精度(double)で演算され、必要に応じて整数(int)に型変換される。
ex.   10*($spstp + 7)/ $mul
それに伴って、これまで、サーボ角度のパラメータを、ふにする場合、新たにパタメータを定義しなければならなかったが、不要になり、使用時にパラメータの前に　'-' （マイナス記号）をつけるだけで良くなった。（構文解析で単項マイナスの処理が面倒なので、冒頭のマイナスの前に0をつけているだけなのだが（笑） -$test の時 0-$testする）
（２）角度 defangles、スピード defspeed、実行 defexec の定義は、キーと値を':'で挟んだものになっているが、値が数式で評価されるために、その中に空白を入れたい場合もある（入れなくてもいいのだが）。その場合は、あたいの全体を [  ] で囲む必要がある。その場合でも':'と [  ] の間に空白を挟んではいけない。
（３）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