RaspberryPI3にアキバで買ってきたUSB DACと片側12.5Wのステレオアンプをつないで音を出してみた。
コマンドラインは、
$ aplay -D plughw:2,0 test.wav
でないとダメだった。DACとアンプをつないだ後、デバイスの指定がplughw:2,0でなければ、ならなかった。
音量調整は、
$ alsamixer -c 2
でする。家でやる限りは、10％程度でよかった。12.5Wのパワーはすごかった。大きなパワーにすると、少し音が割れ気味なのは、スピーカのせいか。
カテゴリー: RaspberryPi
RaspberryPIで二つ目のI2CチャンネルをPCA-9685用に動かす
足郎２では、左右の足を別のスレッドで動かし、かつそれぞれ別のI2Cチャンネルを使うことにしている。
RaspberryPIで二つ目のI2Cチャンネルを動かすことについては、
http://robo.genv.sophia.ac.jp/wp_robot/?p=1645
にだいぶ書いておいた。
ただ、これをサーボドライバPCA-9685用に動かすためには、一つ面倒な作業がある。それは、I2Cのデフォルトである１チャンネルは、すでにRaspberryPIのボード内でプルアップされているのであるが、この第二チャンネルは、プルアップされていないので、外部プロアップをしなければならないのである。
ただ、秋月電子のボードには、プルアップ用の抵抗がすでに取り付けられていて、このJ1とJ2を、直結すればSDAとSCLの二つのピンがプルアップされる。
直結する方法に戸惑ったが、ハンダで無理やりすれば良いのだと了解して、そうしている。今の所不都合はない。
もう一つの倒れるセンサーデータ
少し早足で歩かせて、追加で「倒れるデータ」を取った。
３度も倒れそうになった。基本的に、前に確認したものと同じであり、倒れそうな方向に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などで、片方をロックしたりしてもダメだ。だから、サーボを制御しているスレッドが、センサーの警告を受け止める適切なメカニズムを作らなければならない。
センサースレッドが警告情報をどこかに書き込み、サーボスレッドがそれを細かく読んでチェックしているという状況だと、書き込みのバッティングも起こらない様な気がする。
二足歩行中の3軸加速度センサーのデータ
まだ、スレッド化はしていないのだが、センサーのspiによるシリアル通信がサーボを動かすi2cとバッティングしないかを見るために、ロボットが歩行中に、同じRaspberryPiから、センサープログラムも動かして影響を調べた。基本的に影響はない様だ。独立してデータを処理している。
歩行中のセンサーデータを初めてとったことになる。歩行の様子は、前の記事にあるゆっくりとしたものだ。データは20秒間の動きを10ミリ秒間隔で2000個とった。片側3歩くらいずつ動いている。
横方向（Y軸）は、揺れることが必要なので、まあいいが、縦の動き、前後の動き、X軸の動きの不安定さは、歩行の不安定さに直結しているが、これもまた、ある程度までは必要。
実際に速さを増やして、不安定歩行をさせてデータを取らなければならない。
3軸加速度センサーKXSD9-2050をRaspberryPIのSPIで動かす（２）
プログラムの方も形が整ったので、書いておこう。ロボットの頭頂部につけたセンサーを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;
}
3軸加速度センサーKXSD9-2050をRaspberryPIのSPIで動かす（１）
今日、ほぼ、丸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の基盤の上に、専用の追加基盤がはめてある。黄色が２本使ってあって醜いが、線の色不足である。
続いて、プログラムである。配線にも自信がないときには、プログラムのせいでダメなのか、配線がダメなのか、そもそもボードが壊れているのか、あらゆることに疑いが出たが。なんとかなった。
プログラムについては、次回まとめて書く。
足郎１、歩行の改善
これまでの足郎１の歩行は、途中で大きな回転が入ったり不格好だったが、それを改善するために、これまで二足歩行の１サイクルを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の改訂
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
足郎１を二足歩行させてみる
次の段階として、前への踏み出しをするようにして、二足歩行させてみた。
原理としては、二足歩行であるが、あまりに不恰好だ。膝がないために、足の折りたたみができないので、重心を片足に乗せて、重心のない方の足をしっかり釣り上げなければならず、負担が大きい（下記、サーボ角度図参照）。
二足歩行のために、足の裏に厚さ３ミリの天然ゴムを貼ったことを記しておこう。不要な回転を止めるためだが、役に立った。ただ、それでも回転は完全に止めきれていないが。これも、膝がないことが影響しているか。
サーボ１６個フルに動かしているので、スピード感がなくなってきている。しかし、まだ、スピードを出す余裕はある。
サーボ角度図の最新版は次のようなものである。
動かすためのプログラムを以下に示しておく。プログラミングで、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
######## 以上 ###########