書きこみ順は、新しい物から古い物の順です。
2015年に動かして以降、RCサーボのキャリブレーション（関節パラメータファイルの調整）を行っていないので、各関節が
狙った角度からけっこうズレていて、左右に同じだけ体を傾けたはずが、片方の足裏が地面から少し浮いたりなどありましたが、
とりあえずの確認は出来ました。
足先の軌道で座標を指定するとき、実際のところ水平で平らな場所しか歩けなかったので、
足裏を水平以外の角度にする計算を省いていましたが、搭載マイコンの計算パワーも上がってきたので、傾斜面ぐらいには対応してゆきたいとうことで、
全身の動作確認に先立って２Ｌ１以来省いていた部分を復活しました。
たまにはいつもと違うやり方でやってみようと、油粘土でいくつか作ってみて、気にいったものを 紙粘土で作って見ることにしました。油粘土と紙粘土は共に、近くのスーパーの学用品売り場 で購入した、子供用のものを使ってみました。
油粘土バージョン いろいろ。
結局5番目のやつに決めて紙粘土で作ってみました。
紙粘土バージョン
この紙粘土は、パッケージに「出来上がりが軽い」と強調してあるだけあって、密度高めの 発泡スチロール程度の比重と硬さでした。
油粘土は、匂いで子供の頃の記憶が呼び覚まされる・・・かと思いましたが、船内工事で 壁の電線貫通穴に詰めておく粘土と同じ匂いだったので、前の職場を思い出しました。（笑）
具体的には、遅延が無い場合、１００Hzで回るループにて足先の軌道を１００分割したモーションを、１秒で再生できれば目標達成と 判断しました。
DSPを使わなくても、とりあえず必要な速度が出ている事が確認できたので、より手間がかかる歩行動作の移植に 進むこととしました。
2l5v1_vcには、例えば「腰の高さを一定にして歩く」とか「接地相の脚の膝を伸ばして歩く」とか、複数の条件が違う 歩行モーションを生成するクラスが実装されています。今回の「マイコンの計算パワーを見てみる」目的には、そのうち１つあれば 用が足りるので、統合後のバイナリサイズを小さくする意味でも、冗長な部分を削る作業をしました。
マイコンとRCサーボ（KRS2552R）との通信は1,250,000bpsで特に問題ありませんでした。PCとマイコンの通信に関わる 部分もそのまま行けるかと期待していましたが、前のマイコンのLPC1768の時と同じボーレート921,600bps ではデータの取りこぼしが起きて速度を下げざるをえませんでした。
今回は歩行時の制御がPCからではないので、115,200bpsで調整等を行うことにしました。
ハンガーに吊るして調整中
バッテリーを入れた状態
バッテリーを出した状態
マイコンボードの取り付け状態
バックパックの取り付け状態
取っ手とアイプレート
具体的には腰のヨー、首、腕のヨーの合計４個のRCサーボ（約１６０ｇ）を外して軽くすると共に、肩の位置を 下げて重心を下げる事により股関節への負荷を下げてみる事としました。
その他には、自作の小さめの足パーツでは、温度変化に起因すると思われる、RCサーボのドリフトによる足先の位置 変化を直すための調整をマメに行わねばならず面倒だと感じていたので、キット付属の足パーツを使ってみる事としました。
胴体と腕をつなぐパーツの仮組
バックパックに収まったマイコンボードとI/Oボード
モーションセンサーは９軸のLSM9DS1に変更し、加速度と角速度のICを個別に搭載していたものを１つのICにまとめ ました。
マイコンボードとI/Oボード
KRS2552RHVの動作確認
制御ソフトについては、モーションセンサーのI/Oルーチンは例によってmbedに公開されているものを取込、KRS2552RHV とのI/OルーチンはLPC1768用に使っていたソースコードを取り込んで動作確認まで行いました。
写真に写っているバッテリーは内臓電池として搭載予定の9.9V 900mAhのLifeタイプです。
調べ方としては、「だいたい分かれば十分」ということで、細かいオーバーヘッドタイム等は気にせず下の様なコードを実行し、内蔵タイマー で経過時間を測ってみました。
while(1) {
timer.reset();
timer.start();
for( float x=0.0f; x<10.0f; x+=0.001f){
volatile float y = sin(x);
//volatile float y = arm_sin_f32(x);
}
stop_watch = timer.read_us();
printf("Elapsed time is %5d minro seconds.\n", stop_watch);
led = !led;
wait_ms(1000);
}
LPC1768のsinの場合の約104.3msecをベースとして、Nucleo-F401REでFPUを使ったsinで約11.8msec、
DSPを使ったarm_sin_f32で約6.6msecと、15倍以上の処理速度が出ていました。
２自由度の脚とLPC1768の組み合わせのPen4でリアルタイムの逆運動学計算をしながらの歩行ができたので、これならば ６自由度の脚の2L5もNucleo-F401REならできるかも・・・と言う事で、マイコン換装の方向で検討を進めることとしました。
mbed LPC1768の中の、具体的にどこにシリアル通信の信号を拾える場所があるのかを調べるのが 面倒だったので、オシロで実際に信号を見てみる事はせず、 ＰＣからマイコンにデータを送った後、一定のバイト数毎に１バイトのデータを返させる というのの、「一定」の部分を変えながら様子を見ました。
「OSのバージョンが上がると処理が遅くなるもの」という先入観を持って調べていましたが、よく見ると、 意外にも処理が早くなったせいだという、何と言う事はない結果でした。
訳が分かったので、ソフトで簡易的にフロー制御を行い、1768側でバッファーオーバーランエラーが 起きないように調整し、また元通り動けるようになりました。
歩行モーションを詰めてゆく方は、ひとまずKHR3HVのハードの性能としては「まあこんな所かな・・・」というところまで 出来た気がしているので、通信の問題調査と対策は、そのうち、気が向いたら着手と言う事にしておきました。
手の製作
つま先とかかとを使う足先軌跡
足先軌跡表示プログラム
以前の動画を見比べた感じでは、効果があったようです。
もっと良くするためにということで、遊脚相で（グラフの青い部分）足首のピッチ軸とロール軸について、KONDOサーボの設定項目の ひとつである「ストレッチ」パラメータを小さくして、位置保持力を下げてみました。(設定値 60 --> 8)
すると、不安定で歩けなくなってしまいました。赤い部分の始めと終わりでは、足首に力が入っていないといけない様でした。
見ていると横に転倒していたので、ピッチ軸のみ保持力を下げてみたところ、不安定な感じはしますが、数歩で転倒する事はなく、 足音が少し静かになりました。うまくやれば効果はあるようでした。
逆に保持力を上げてみたところ、歩行の安定が良くなりました。結局のところ、サーボのパワー不足で、計算で狙った通りに動いていない ということだろうと思いました。
手の制作−指の試作
手書きの大体原寸スケッチと、ＣＡＤで描いた図面です。
指の関節は、普通に軸受と滑車を内蔵したタイプと、板バネを使ったタイプを考えています。始めは板バネの方を試作してみました。
とりあえず、これでも良いかな〜と思いました。
その他、今週末は歩幅をいろいろ変えて歩かせて様子を見たり、歩行モーション生成プログラムのソースコード の一部をサブルーチン化したり、変数の使い方を統一したり、試行錯誤途中の意味のないコードを消したり等々、見やすく清書しました。
14.10.12の書き込みの時点では、一歩で加速できたのは歩幅60ｍｍ程度まででしたが、今回は100mmでも平気でした。 自重による膝関節の目標角度からの偏差が減り、足先の位置制御の精度がよくなったせいかなーと思いました。
一歩で加速するには歩幅が大きすぎた様で、歩き出しが不安定です。以前の腰の高さ一定 のモーションと 比べると、接地のショックが大きいためか、頭のブレも大きくなっていますが、膝を深く曲げないので、 膝の負担は、接地のショックを別とすれば、減少傾向の様でした。ショック低減の方向で改良しようかと思いました。
横から120fpsで撮影したスローモーションのムービーです。
後ろから30fpsで撮影した通常スピードのムービーです。
下がそのムービーです。足を地面に下ろすときも踵からになっていますが、これは意図したものではなく、 膝が自重で制御目標の角度よりも深く曲がり、体全体が後ろへ傾いているためです。
120fps（１／４倍速のスローモーション）
以前の歩行モーションでも十分関節可動範囲に収まる、歩幅＝５０ｍｍで歩かせています。
股のヨー軸と膝が自重で制御の目標よりも深く曲がるため、足が地面から離れるタイミングは遅めに、
足が地面に着くタイミングは早めになる傾向のもと、両足接地状態の時間を長くしたので直進性が低下しました。
見た目だけでメリットが感じられない結果ですが、しばらくいろいろ試してみようと思っています。
遊脚側の足が地面から離れる距離が、制御目標に近づいたので、膝サーボの負荷を減らす目的は達したようです。
ただ、歩く音が大きくなり、体全体への衝撃が大きくなったみたいで「ロボット全体に負荷が大きくなった？」様な感じでした。
接地の衝撃を吸収するという観点で、踵から接地するか、足の裏に何か張るか等してショックを減らすことを考えた方が良さそうです。
120fps（１／４倍速のスローモーション）
歩幅100mm、通常スピードで撮影したムービーです。机の上でテストするのは手狭な感じです。
正常に働くコマンドと働かないコマンドの差、ボーレートを半分に落としてみる、２２軸分の角度データを 一気に送っているところの途中にSleep(1)関数をいくつか入れてみる・・・などなどで色々調べてみました。 その結果、ＰＣが速くなったので、同じボーレートでも、送信データの１バイトと１バイトの間隔が短くなり、データの 取りこぼしが起きていると結論しました。
921600bpsで、だいたい22バイト連続して送ると調歩同期の同期がずれることが分かったので、10バイトに 一回、マイコンから１バイト、ACK替わりのデータを返させ、ＰＣ側はそのデータを読んでから 先に進む様にしたところ、また歩けるようになりました。
かなり効率アップが図れたので、ＫＨＲ３ＨＶが組みあがってすぐにやればよかったな、と思いました。
調整スタンド全体
１２ｍｍ厚のベニア板をヒノキの棒で支えています。切り欠きの部分にＫＨＲ３ＨＶの胴体をセットします。
ＫＨＲ３ＨＶセット状態
行うのは脚関節の調整だけなので、腕は脱力して上に向けてセットします。
スタンド裏面
腰の部分のサーボを軽く挟み込むようにして、定位置を保つようにしました。
多少反っているような板と棒で作ったので精度は良くないですが、吊った状態で適当に定規を あてて調整したいたので、台の床面から三角定規を立てて位置合わせや、足先移動量の計測を すれば、まえよりだいぶマシな、再現性を含めた測定が簡単になるのでは、と期待してます。
ジグソー（電動工具）
最近、ノコギリを使った簡単な工作でも腕や腰が痛くなることが多いので、楽に作れるように
ジグソーを買い、使ってみました。ＤＩＹ用ではまんなか辺の価格帯の機種を選びました。
楽に切れるので、もっといろいろ作ってみようかという気持ちが湧きました。
２Ｌ１／２Ｌ２ではＣ＋＋で作ったコンソールアプリの方にこの機能を付けていましたが、今回は操作性を 重視して、ＶＢで作ったプログラムの方に付けました。
画面イメージ
この例では、イメージの下の方に並んでいる数字で、ＩＤ１１のサーボの-90, 0, +90度に相当する ＲＣサーボに与える数字を調整しています。
面倒で楽しくない作業なので、効率的かつ、なるべく自動でできる方向で検討を進めようと思っています。
足の裏に貼りつけた、0.3mm厚のクロロプレンゴムのシートです。1994年ころ、Ｒ魚の尾びれ用に、いろいろな厚さと種類のゴムシートを
買ったものが残っていたので使いました。現状のプラ板製の足の縁のＲにきれいににフィットするよう、手持ちの中でも薄いものを選びました。
貼りつけ前後の写真です。後で違う材料も試せるよう、撤去が比較的容易なゴム用の両面テープで貼りつけました。
動画のリンク１（改善前 120fps）
歩きだして定常状態になれば自分の足を踏まなくなっていることもムービーで分かったので、その点に着目して見たところ、一歩目は 股関節のロール軸の負荷が、定常状態と比べて大きい事に気付きました。 言いかえれば結局のところ、股関節のロール軸サーボのパワー不足が原因なので、最初の一歩で足を上げるタイミングを定常状態と同じにして、負荷を下げたら解決しました。
動画のリンク１（通常速度−30fpsで撮影）
動画のリンク２（120fpsで高速度撮影）
安定がいまいちでしたが足の大きさ70mmに対し、歩幅80mm（周期は前回と同じ１秒）までいけました。そろそろ足の可動範囲の境界にかかるので 足の辺（踵とつま先）も使い、もう少し長い歩幅で歩けるようにしてゆこうと思います。
「まあこんなもんかな〜」といった程度には出来たので、先に進めます。
動画はよく観察できるよう240fpsで撮影しました。ジャイロのフィードバックは無です。 KHR3HVと比べると、剛性と各関節のトルクが低い２Ｌ１用に調整したモーションで、このロボットに 適したモーションで無かったです。がっかりでした。
それはさておき、移植の作業をしながら約１０年前に２Ｌ１でどういうことを試したのか、色々なバージョンの ソースコードに目を通しながら思い出しつつ、新しく作るプログラムの構想を練りました。
今日時点でだいたい考えがまとまったので「膝の屈伸」と、直進の「初めの一歩」のプログラムから コーディングを始めました。今までのプログラムに目を通した結果、結局「いろいろ試してうまくいったことを きれいに清書してある」形のＰｅｎ４のコードをベースにしました。
少し腰を落として、常に足裏全面を接地して歩く、いままでおなじみのモーションを作ったのち、接地脚の膝は 伸ばし、遊脚相の初めと終わりにつま先と踵を使うモーションへ進もうと思います。
単体の電池。
タブをはんだ付けしたところ
パックとコネクタ取付完了。一緒に写っている変換コネクタは、充電用です。
スイッチの取付状態（バックパック外側より）
スイッチの取付状態（バックパック内側より）
その後、左右の足に同じX,Y,Z方向に動かす入力を与え、ズレ（動かすにつれて、つま先が近付くとか離れるとか）を見ながら関節 パラメータファイルの数値を調節しました。 ここもスムーズに歩くために重要なポイントなので、後日関節パラメータファイルを編集する機能を作ったのちもっと、座標の絶対値で 調整する予定です。
ここまでで、ひとまず足先を座標入力で動かせるようになったので、足先の軌道を生成する（言いかえると歩行モーションを生成する） 関数（create_waveset）を移植しました。移植作業としては、coo2servoまでで、引数を少し整理したので、それらを反映しましたが、 だいたいそのまま２Ｌ１のプログラムから持ってきただけでした。
次はジャイロのフィードバック機能を入れたのち、２Ｌ１の歩行モーションを試してみようと思います。
２Ｌ２の同名の関数を移植しましたが、４節リンクが無く、２Ｌ２に比べて、軸のオフセットがある個所が少ないので、大分シンプルになりました。
関節パラメータファイル、ファイル作成前に描いた図、移植した関数の間で記載内容の整合性を取りながら、coo2servo関数をテストというか検算中です。
だいたい良さそうなので、次は歩行のモーションを生成する（実際はcoo2servoに与える足先の軌道を離散化した点列を生成する）クラスを移植する予定です。
そこまで行くと、とりあえず２Ｌ１の様な感じで歩けるようになります。その後はボディーの性能を生かして、より大きな歩幅で、早くスムーズに歩けるようにする 方向で作っていこうと考えています。
うまくまとまったら、ロボットにBeagleBoneBlackの様な、UNIX系OSが動くボードを載せて、それに移植して動かそうと 思っているので、CLRは使わず、比較的移植しやすい（と思っている）Win32のプロジェクトにしました。
実は、Visual C++ のWin32でシリアル通信機能そのものに関するプログラムを書くのは初めてだったので時間がかかりました。 プログラミングにあたっては、以下のサイトを参考にさせていただきました。
Windows デベロッパーセンター [VC++ Native] 仮想 COM ポート アクセス
http://code.msdn.microsoft.com/windowsdesktop/COM-howto-4b79a479
YS電子工作ラボ RS232C シリアル通信
http://www.ys-labo.com/BCB/2007/070512%20RS232C%20zenpan.html
2l5v1_vcから関節を動かすことが出来たので、以前のロボット用のソースコードから、幾何計算や設定ファイルのIOルーチンなど、流用できる コードを探し集めながらコーディングを進めています。
今日のところはこんな感じです。
各関節の可動範囲を調べつつ、KHR3HVを４足歩行させるとしたら、どんな姿勢が良いかな〜と考えていました。 仰向けで４足歩行させるのが、関節の可動範囲的に良さそうです。そのうち４足歩行のモーションも作ってみようかと思いました。
こういう情報や図は、歩行モーションを計算する時などに必ずいるので、キットのＣＤに入っていたら嬉しかったかも・・・と思いました。
この図を描いていて気がつきましたが、腰、股関節、肘が軸同士が直交せずオフセットがありました。これが無ければ計算ステップが その分減り、プログラムがシンプルになって良かったのに・・・と思い、少し残念でした。
KHR3HVはサーボモーターのケースやフレームに印がついていて、なかなか調整が楽に出来ました。
ＰＣ→LPC1768間のボーレートを921600bpsにセットして、だいたい１秒間に１８０回程度更新できました。体を
支える事が出来るのか見るため、簡単な膝の屈伸を行うモーションを作り動かしてみました。
見た感じでは特に問題無く、ＰＣからの制御でも歩けそうな感じだったので、モーションセンサの情報などをフィードバックする
通信の時間なども見込んで、１秒に１００更新周期でこの先しばらく動かしてみようと思いました。
かなり余裕を見た設定ですが、Ｐｅｎ４号までは５０回だったことを考えると、２倍の周期からのスタートなので、悪くなかろう
と思いました。
テスト中のスナップ
パックにはずいぶん前に購入したダイソーの物干し竿カバーを使いました。
また、信号の衝突を避ける意図で入れたトライステートバッファ(74HC125)の切替タイミングが 早すぎで、受信完了前に方向を切り替えてしまっていたせいで情報を受け取れていないことが 分かったので、これもwaitで調整しました。
以上で受信もうまくできるようになりました。
数年前の引っ越しの時に、古いアナログオシロスコープが使用頻度も低く邪魔だったので捨てて しまい、次に使う用事が出来たときに新しいのを買おう！と思っていたので、今回、新しいのを 買いました。TextronixのTBS1052Bという機種です。以前、同程度の性能？のTDS210というオシロ借りて 使っていましたが、それと比べて随分安くなったな〜と思いました。まだ使い始めたばかりですが、 とくに違和感なく使えています。
マイコンボード
ICS3.5のマニュアルや、KONDO Webサイト内の関連するページを参考に動かしてみたところ、通信速度を1.25Mbpsに セットしてmbedLPC1768から動かすことが出来ました。ただいろいろ試すも、情報の読み取りができず調査中です。
情報を読めなくともとりあえずは困らないので、サーボをスレーブモードに、情報をサーボから返さない様にセットして おきました。
形については「なんかカッコいいやつにしたい・・・」という気持ちはありますが、早く動かして遊べる状態にすることを 優先して、とりあえずタダの箱としました。このへん何を優先して妥協するかは製作者の性格が出るところなのでしょうね。
先日Pen4号に付けたのと同じ、STマイクロのL3GD20とLIS3DHの組み合わせです。
ネットで見た感じ、KHRとの組み合わせでよく使われているようなので、ALINCOのDM-33OMVにしました。
以前のKONDOのRCサーボは、電源が入っていない状態では、出力軸がとても軽く抵抗なく動いたので、
それで組んだロボットも、電源が入っていない状態では糸の切れた操り人形みたいな感じで、クタクタでした。
KHR3も電源が入っていない状態で立たせることは出来ないだろうと、なんとなく思っていたので、
こんな感じで立たせることが出来る事が意外でした。教示機能が使いやすいという観点では良いと思いました。いっぽう、
ゲインを下げたり、不感帯を広げてパッシブに動かしたいケースでは、前の方が良かったのでは？と思いました。
そうえいばPen4号の足も、本の作例よりも工作精度が低かったり、調整が甘かったりしても歩けるように、ベースとなったＵｎｉよりも 一回り大きな足にしたのを思い出しました。
パッケージ
Pen4号のSX-101ZとKRS2552で、サーボの大きさ比較。大きさはだいたい同じ・・・だけれど、出力と消費電力が４倍くらい違います。
組み立て中のスナップ
どういうコンピュータを載せるか、まだ具体的な設計案はありませんが、Cortex-M3かM4のマイコンとBeagleBoneBlack(Cortex-A8)の 組み合わせなんかどうだろう・・・とか考えています。
