G-Tune 2004FI SIPHA
SYSTEM
SIPHA CORE
ハードウェア あまり変わり栄えしませんが、SIPHA
COREの回路図はこんな感じです。今回も例によってすべてソフトドライブで、サーボ制御に始まってR/C受信機取り込み、各種センサ取り込み、全部H8/3664でやることができました。後、いろいろありまして、サーボとの間には1.5KΩの抵抗が入っていますが、次回作る時は、多分、5KΩぐらいにしようと思っています。この件に関しては、現在、調査中で、またそのうちアップします。
クリックで拡大
電源回路 このレギュレータ、秋月電子で購入したのですが、なぜかカタログページの5Vから検索すると出てきません。「レギュレータ」で検索をかけると出てきます。今は、表面実装で低損失のものもあるので、そっちの方がいいかもしれません。また、0.1μFのコンデンサはチップ部品を使っています。実際に使ってみて思ったのですが、チップ部品、結構、ユニバーサル基板と相性が良くて使いやすいです。
昇圧回路 実戦時には結局使用しなかったんですが、今回、MAX631というICを使ってステップアップ回路を組みました。部品一式は「若松通商」でそろえることができます。以前、レポートしましたが、入力が3.5Vぐらいでも5V出力、2.5Vでも3.7V(30mA時)でますので、H8/3664を動かすぐらいなら大丈夫そうです。回路図をを見ていただくとわかりますが、マイコン、加速度センサ、R/C受信機は昇圧後の電源を使っており、距離センサはレギュレータから直接取っています。これは、距離センサが結構電気を食うためです(MAX100mAぐらいかな?)。 実戦で使用しなかった理由なんですが、以前、原因不明のCPUハングアップが発生した時にはずしてそのままにして忘れていたのが理由です。晴れてハングアップの原因ではないことがわかったので今後、生かして使ってみようと思っています。でも、無くて問題が無かったといことは、必要ない?
(やっぱりはずそうかな…)
電源電圧監視回路 4.7KΩの抵抗で生電圧を1/2にしてH8/3664のAD端子に入力しています。0.1μFのコンデンサは必ずつけましょう。前回、これを付け忘れて読み取りができませんでした。今回は、ちゃんと忘れずにいれました。今回は読み取って、スクリプト処理の時に使っているだけですが、動作作成時など、急激な電圧変化を監視していれば、サーボロック等の症状を発見できるようになるかもしれません。
3軸加速度センサ 前回、ADXL202を使っていて今回、ACB302に変更した理由ですが、実は「ADXL202を水平に取り付ける場所が無かった」からです。そのため、どの向きでも取り付け可能な3軸が必要になり、これをつけました(G-Tuneサイズならではの理由)。アナログ入力はやったことがなかったので、いろいろ不安でしたが、起き上がり判定ぐらいには使えています。起き上がり操作は、どんな向きに倒れていても同じ操作で、SIPHA
COREが判定して起き上がってくれます(えらい!)。 配線取り回しの関係か、未だに信号が結構大きくフラフラしています。これでいいんでしょうか…。
PSD距離センサ G-Tuneの両腕についているやつですが、10μFがかましてあるのは、千石電商のホームページで「入れたほうがいい」と書いてあったので、アナログに弱いSISOとしては、「ま、悪いことはなかろう」ということで入れてます。
R/C受信機 前回使っていたGWSのを使いまわしています。
シリアルEEPROM シリアルEEPROMは、H8/3664が持っているI2Cモジュールを使用せずに使っているため、普通のポートに接続されています。なぜか本来モジュールがついているP56、P57に接続するとうまく通信できません。1つの理由としては出力電圧が他のポートより低いというのがあげられると思うのですが、だからと言って…ね〜?そんなわけで、P56、P57は、入力ポートとして使っています。
SIPHA
COREのソフトウェア構成 これまたあまり変わり栄えしませんが、SIPHA
COREのソフトウェア構成はこんな感じです。マルチタスク風に書いてありますが、実際にはシングルタスク+割り込み処理です。ただ、こういうふうに整理して作ると、何かとメンテナンスとかチューニングがしやすいので、いつもこのように作っています。前回、完成し切れなかった部分が、ようやく完成しました(ついでに悪いところも見つけてしまったけど)。
センサ処理系を除くそれぞれのモジュールは、サーボ制御周期を基本として逐次呼び出すようになっています。ほら、サーボ補間計算とかって、次にサーボ制御信号を出す時までに計算すればいいですよね。また、Action
Scirpt実行処理もそうです。で、センサとかR/C受信機処理を逐次行っているという感じでプログラムされています。
クリックで拡大
RCI (R/C receiver Command
Interface) R/C受信機からの信号処理は、IDSによって処理されてプロポのスティック位置として保存されます。このモジュールは、その情報を元にして次に実行するAction
Scriptを決定します。前回、モードなどを意識した、ちょっと凝ったつくりにしていたのですが、今回はシンプルにコマンド毎に実行するAction
Scirpt番号を固定し、ただ単純に、条件がそろっていれば新しいAction
ScriptをASMに要求するという構造にしました。ただ1つ、実行レベル(プライオリティ)制御が入っており、現在実行中のAction
Scirptの実行レベルが、新たに実行するAction
Scriptのレベルよりも高いときのみ上書き(オーバーライト)実行します。 この、「実行レベル」は、Action
Scriptにあらかじめ定義してあるものです。これによって、「待機中ループ処理」から「歩行」などへの動作へ移行できるようにしてあります。また、「歩行」などの動作が完了したときは、当然、プロポのスティックはセンターに戻っているわけで、これによって、また「待機用Action
Script」を実行開始します。「待機用Action
Script」の中には、前の姿勢からの移行処理や、待機中に行う処理(今回はカラですが、次回は自動バランスとか書きたいです)が書いてあり、ループして待つようになっています。 もし、姿勢移行などでどうしても他のAction
Scriptからの上書きを許可したくない場合は、動的にAction
Scriptの中で実行レベルを操作することができますので、「待機ループのところは実行レベルを低く」して「姿勢移行中の時は実行レベルをあげる」ということができます。ここが今回、手を抜いたというか、後で気がついて、操作ミスに泣かされたところです。
SOI (SCI Operation
Interface) SCI通信モジュールです。SIPHA
TERMからのコマンド解析が主な仕事で、 メモリの読み書き、シリアルEEPROMとの読み書きを中心として、SIPHA
CORE自体のモード変更機能などを持っています。上のソフトウェア構造図を見ていただくとわかりますように、各モジュール間のインターフェイスは、メモリで介在するようになっています。つまり、このメモリエリアにデータを書き込むことにより、別モジュールから要求したが如く振舞えます。SIPHA
TERMからサーボを動かしたりする場合、本来、SPCからSPGに要求することでサーボ動作するようになっているところを、このSOIモジュールを介して、メモリに要求内容を書き込むことで動作させれるようになっています。 また、極力サーボ制御周期などに影響が無いようにするために、送信処理などは結構、複雑になってしまいました。
DAL (Device Access Library) シリアル通信などのライブラリです。
IAL (I2C EEPROM Access
Library) シリアルEEPROMアクセス用のライブラリです。前回、H8/3664に内蔵されているI2Cモジュールを使用してシリアルEEPROMにアクセスするようにしていたのですが、今回は自前で用意しました。結果として、ぐっと通信速度は下がりましたが、サーボ制御信号生成などの割り込み処理と併用しても問題が発生しなくなったため、「隙間を縫ってアクセス」等の処理が不要となりました。
今回、やり忘れたのですが、次回はDALに統合される予定です。
ASM (Action Script Manager) Action
Scriptの管理モジュールです。実行要求に従って、シリアルEEPROMからメモリにAction
Scriptデータをロードし、コマンドを解析しながら実行します。電源電圧によるAction
Scriptの切り替えや、加速度センサによる姿勢判定等もここで行い、Action
Scriptの中でジャンプしたりするようになっています。 メモリのロードエリアは、250レコードで、大体、事前審査動作の起き上がりを除いた全ての動作が一発で動作させれるぐらいです(もちろん、カウンタによるループも可能なので、それも併用しての話ですが)。Action
Scriptから別のAction Scirptにチェーンすることも可能なので、長いAction Scriptを書く場合でも、Action
Scriptが別れるだけで、問題になることはありません。また、フルにロードしても、数十msec程度でロードしますので、見た目には、ほとんどわかりません。 現在、32KバイトのシリアルEEPROMを実装していますが、約7000レコード記録可能で、今回のG-Tuneの動作でも5000レコード使用しているだけです。
IDS (Input Device
Scanner) R/C受信機、加速度センサ、距離センサ、電源電圧の情報を、SIPHA
CORE内で扱えるようにするためのモジュールです。実体は、通常の関数として呼ばれる部分と、TimerVによる0.128msecの割り込みよって動作する部分に別れており、サーボ制御信号生成中は通常の関数として呼ばれて信号解析を行い、軌道計算などをしているときは割り込み処理によって信号解析を行います。これについては、後で、全体の制御周期の説明をしますので、そこで切り替わりポイントを説明します。 また、SOIからの要求により、R/C受信機取り込み処理を中止し、変わりに、SOIからダミーのデータを設定することで、ROIやASMがチェックする「プロポのスティック位置」をSOIからコントロールできるようにすることができます。J-ClassでシリアルコントロールでG-Tuneを動作させたときは、この機能を使用しました。
SPC (Servo Position
Controller) サーボの補間動作が主な仕事です。ASM等から、「何度から何度まで、何クロックで動作」という形で指示が入りますので、これに応じて補間計算、及びサーボの角度からパルスへの変換、XZ座標系からのパルスへの変換を行います。
HIP (High-speed Integer
Proccessor) sin、cos、atan、rootの計算を行うモジュールです。三角関数については、テーブルによって計算を行っています。また、rootについてはニュートン法を使って計算してます。このモジュールによって、「XZ座標から各サーボの角度計算」などを行っています。
SPG (Servo Pulse
Generator) サーボパルスを発生するモジュールです。実際にはTimerWの割り込みルーチンです。例によって、4本で1処理としているのですが、2本づつ、0.5msecずらして出力するようになっています。これは、突入電流を減らす対策です。G-Tune
2004FIは単4×6セルで動作させるので、どうしても同時に取り出せる電流最大値が低くなります。そこで、もっとも電流が流れる「起動の瞬間」を少しでも小さくするために、このようにしました。 基本的なプログラム自体は、「最初に信号ON」しておいて、「GRA〜Dのコンペアマッチで信号OFF」するものです。完全ではないのですが、サーボジッタを極力減らすようになっています。具体的には、この4つのサーボ制御信号のOFFタイミングをあらかじめチェックし、前の割り込み処理によって、次の割り込み処理が遅れる可能性のある割り込みを探します。で、もし遅れる可能性がある場合(OFFタイミングが近い場合)、前の割り込み処理の中で次の割り込み条件が成立するのを待ち、条件成立したらそのまま処理するようにしています。よって、次の割り込みは、前の割り込みに吸収された形になります。
と、まあ、基本的なオペレーティングシステムをプログラムで構築しておき、後は、Action
Scriptでなんとかするという形で実装しています。それだけに、今回は、さらにAction
Scriptの機能を追加して、「距離センサ」、「電源電圧」、「加速度センサによる姿勢」、「変数」を扱えるようになっています。
気になるプログラムサイズですが、オプティマイズレベル2で25Kバイトほどです。まだまだ余裕がありますよ〜(これを余裕があるといっていいものかは疑問だけど…) 。
制御方法概要 基本的にはサーボ制御周期である20msec(本当はもう少し速くして16msecぐらいにしたいんですが…)を基準として、R/C受信機パルス、各センサーの値の読み込みを行い、それに応じてAction
Scriptを実行するようになっています。それがグルグルまわっているだけです。今のところ、すべての動作はAction
Scriptによって設定しています。
1制御周期は、20msecで実行しており、サーボ制御信号出力フェイズと軌道計算フェイズに分かれています。サーボ制御信号は、TimerWの割り込みを使用して出力しています。また、プロポ入力はパルス値ですのでカウントする必要がありますが、これは、TimerVの0.128msecのカウントアップによって行っています。後で説明しますが、TimerVの割り込みとTimerWの割り込みが同時に発生すると、サーボの位置決めが狂いますので、TimerVの割り込みは、実際には軌道計算フェイズでしか使用していません。さらにこれにシリアルEEPROMの読み込みが入るわけですが、これは、読み込み中はプロポ入力信号の解析を中止して、読み込みを行っています。
サーボ制御信号出力フェイズ TimerWを使用した、4本1セットのサーボ制御信号出力です。アナログサーボは、どうも制御信号を入れたタイミングでモータに電流を流すようなので、0.5msecで2本ずつに分散するようにしています。モータってのは、起動させる瞬間がかなり電流食いますので、
これの対策です。たぶん、デジタルサーボではあまり意味が無いと思います。 サーボ制御信号は0.6〜2.4msec程度、これに0.5msecのずらしが入りますので、4本のサーボ制御信号は3msecで一通りの仕事が終わることになります。これを5回繰り返して20サーボの制御信号出力を行っています。 現在、全体で、20msecの制御周期になっていますので、できればこれをもう少し短くしたいと考えています。アナログサーボの場合、若干ですが、20msecより15msecの方がトルクアップするからです。
軌道計算フェイズ 残りの5msecで、R/C受信機からの信号による次動作の決定、サーボ軌道計算(角度補間、座標補間)を行います。座標補間の方が結構ヘヴィで、登録時、角度情報から現在の座標を計算しているのですが、
全てのサーボ+脚の座標登録を行うと、5msecで納まらなくなってしまいます。このため、Action
Scriptの方で、1Clock(=20msec)ずらして登録してごまかしています。より使いやすいシステムを目指すため、このあたりの処理速度改善も今後の課題です。
R/C受信機の信号検出 R/C受信機信号はサーボ制御信号同様、パルスです。これをTimerVを使って0.128msec毎にカウントしています。ただし、普通に割り込みでやってしまうと、サーボ制御信号処理とぶつかった時に、ジッタのモトになります。そこで、サーボ制御信号出力フェイズでは、メインルーチンのほうでぐるぐる回ってTimerVのオーバーフローを監視してカウントアップ実行し、起動計算フェイズでは割り込みを使ってカウントアップをしています。
シリアルEEPROMの読み取り それでは、シリアルEEPROMはどうやって読み取っているかといいますと…っと、その前に、いつ、シリアルEEPROMを読み取っているかですが、「起動時に初期設定パラメータを読む時」、「R/C受信機から新しいAction
Script動作指示が出た時」、「実行中のAction Scriptから、Action
Scriptのジャンプ命令が実行された時」というタイミングになります。 通常の制御中は、Action Script絡みのみ発生します。Action
Scriptデータは、もっとも大きいもので1KByte程です。 で、読み始めたら…思い切って、サーボ制御信号処理以外の処理はすべて停止し、シリアルEEPROM読み込み処理に集中します。実際、最大で30msec程度ですので、操作上、気になるようなことは無いです。R/C受信機処理が、誤操作防止のため、最短で60msecでシステムに反映されますので、これ以下ならば、影響はありません。このあたりの割り切り方が、妙に自分らしいと思ってしまったりします。
加速度センサ、距離センサの読み取り 今回、どちらのセンサもアナログですので、先のR/C受信機信号検出で使用しているオーバーフロータイミングでスキャンモードを切り替えて読み取りをしています。H8/3664には、アナログ変換をモジュールで自動的にやっておいてくれるスキャンモードとう機能があります。ただ、このスキャンモード、一度に4chしかできません。この場合、加速度センサ3ch、距離センサ2chということで、5chとなり、泣く泣くこうやっています。 実際には、加速度センサ、2ch分(前後左右)しか使用していなかったりするので、これをはずしてもいいかな〜と思ってたりもします。もともとの予定では、倒れ判定の正確度を上げるために上下方向の値も読み取っていました。
ターミナル側は、今回、フリーの開発環境である、WideStudioにてグラフィカルなものを作りました。シリアル通信などは、WIN32APIでベタベタとコーディングしています(WideStudio+WIN32APIでどうやってシリアル通信処理をするかは、TSC16 を見てください)。
また、SIPHA TERMでの、マシーンメンテナンスですが、
Device Adjustment Inspectorで、サーボなどの補正値調整
補正値をシリアルEEPROMへ書き込み
Servo Position Controllerで動作ポーズ設定
Servo Position Recorderで連続動作記憶、Action Script File出力
Action Script Managerに出力したFileを登録し、テキストエディタで時間等のパラメータを調整
Action Script ManagerからRAMに転送し、Action Script実行&テスト
Action Script ManagerからシリアルEEPROMに書き込み
Propo Operation Emulatorにて擬似プロポ操作で動作テスト
とう流れで行います。以下に、各画面の説明を入れますので、参考にしてください。
Main Function Window
SIPHA TERMを起動すると、まず、このウィンドウが表示されます。このウィンドウから、「計画設定」等を読み込み、SIPHA
COREとの通信を開始します。「計画設定」には、動作フォルダ(ディレクトリ)とか、使用するCOMポート、サーボ定義等の情報があります。「電源電圧」が「77」ってなってますが…7.7Vの意味です。直すの忘れていました。
こだわったのが、「緊急停止」ボタン。これを押すと、即座に「サーボ制御信号」をすべてOFFにします。「うぉ、やべ!ど〜しよ〜!?」と思ったときにこれを押すと、「完全脱力状態」になります。ただし、アナログサーボ(+一部のデジタルサーボ)のみ有効な機能です。これは非常に便利ですので、ぜひつけましょう。
Servo Position Controller
SIPHA
TERMの中で一番目立つ画面がこれです。サーボの位置をスライドバーによってコントロールするためのウィンドウです。ここでは、角度指示、及び脚ピッチ軸のXZ座標指示(画面の下のほう)を行います。この画面自身は、ただサーボを動かすだけです。また、背景、各パーツ位置は、「設定定義ファイル」にて設定可能になっています。
実は…指示することに割り切って作りすぎたせいか、後に説明するAction Script Managerとかで、Action
Scriptを実行した後は、自動的に更新されません。このあたり、使いにくいので今後改良したいところです。結構、全体を長期間に渡って「ちま、ちま」と作っていったため、作りに統一性が無く、このあたりの改良が難しくなっています。これから、コードを整理しようと思っています。
Servo Position Recorder
地味な画面ですが、実は非常によく使う画面です。これは、先の「Servo Position
Controller」で指示した角度値を記憶していく機能を持ちます。「Servo Position
Contorller」で「ちょいちょい」とサーボ位置を調整し、このウィンドウで「位置追加」とやると、それが記憶されます。また、矢印ボタンを操作すると、その動作状態を1ステップ戻したりすることができ、再び、そこを修正したりすることができます。
そして、最後に、「Action Script 変換出力」をすると、「仮」のAction
Scriptが出力されます。「仮」というのが、一応、そのままでも動作するのですが、動作時間とかが適当なデフォルト値が入っているので、そのあたり修正が必要ということです。
Action Script Manager
このウィンドウは、Action Scritpを管理するためのものです。Action
Scriptは、パソコン側にテキストファイルで記述されており、これをSIPHA
COREのRAMに転送したり、シリアルEEPROMに書き込んだりして実行しています。よって、「編集用のオリジナル」はSIPHA
TERM側に、「最終的に実行されるもの」はSIPHA
CORE側にあることになります。今までsシリアルEEPROMの中身が事故で消えたことはありませんが、消えても、SIPHA
TERMから再書き込みするだけですので、悲しむことはありません。
Propo Operation Emulator
プロポ操作を真似るウィンドウです。SIPHA
COREの方で、シリアル通信によって「プロポで指示されたのと同じ操作をする」機能がありますので、これに対してコマンドを送ります。これで、「操作系」のテストを行います。プロポを使わなくていいので、プロポの電池節約とか、電波が出せない場所でのテストに大活躍です。
Device Adjustment Inspector
各機器の調整をする(要は、角度変換とかなしで、サーボを直接操作するとか、CORE内部のデータを見るとか)画面です。実は、完成度が低くて「その時その時、必要な機能だけを、それっぽい枠にほおりこんだ」という感じでして…。これから、また整理していきたいと思っています。
Serial Communication Manager
シリアル通信ウィンドウ…になる予定だったのですが、ただのログウィンドウとなっています。当初、このウィンドウに他のウィンドウからいろいろ要求することにより、通信を行うという構想があったのですが、長期間に渡ってプログラムをしていた結果、「初心」を忘れてしまいました。で、だんだんバラバラになってしまい、現在に至ります。
すごい手抜きですが…Action
Scriptも載せておきます。コマンド名とかちょっと変わっていますが、基本的には前回のものをブラシアップしたものです。
EXIT 終了 (Exit)
EXIT=
CLCK 待ちクロック設定 (Clock)
CLCK=n
n: クロック(サーボ制御周期の待つ回数)
PLST サーボ制御パルス設定 (Pulse Set)
PLST=s,s,s,s,s,s,s,s,s,s,s,s,s,s,s,s,s,s,s,s
s: ON、OFF、OF
RCRJ リモートレコードジャンプ (Remote Record Jump)
RCRJ=Stick, Stick, Stick, Stick, EqlJump, NeqJump
Stick: LOW,HGH,CNT,IGN (左縦、左横、右縦、右横の順)
EqlJump: 条件成立時レコードジャンプ(0の時は次のレコードへ)
NeqJump: 条件不成立時レコードジャンプ(0の時は次のレコードへ)
EXST 実行レベル設定 (Execution Level Set)
EXST=n
n: 実行レベル(Action Scriptを上書き実行するための
評価値で、0なら新しい要求があれば無条件に上書き
実行、1または2の場合は、実行中のレベルを越える
場合、上書き実行できる)
PBRJ プッシュボタンジャンプ (Push Button Record Jump)
PBRJ=n, EqlJump, NeqJump
n: プッシュボタン番号(0,1)
EqlJump: 条件成立時レコードジャンプ(0の時は次のレコードへ)
NeqJump: 条件不成立時レコードジャンプ(0の時は次のレコードへ)
LDST LED操作
LDST=n, s
n: LED番号(0のみ)
NIRJ レコード条件無しジャンプ (Non-if Record Jump)
NIRJ=n
n: レコード番号(1〜)
NIAJ アクションスクリプト条件無しジャンプ (Non-if Action Script Jump)
NIAJ=n:
n: アクションスクリプト番号(1〜)
VLOP 変数操作 (Value Operation)
VLOP=Cmnd, n, v
Cmnd: SET,INC,DEC
n: 変数番号(0〜)
v: 変数値
VLRJ 変数レコードジャンプ (Value Record Jump)
VLRJ=n, v, EqlJump, NeqJump
n: 変数番号(0〜)
v: 比較値
EqlJump: 条件成立時レコードジャンプ(0の時は次のレコードへ)
NeqJump: 条件不成立時レコードジャンプ(0の時は次のレコードへ)
VLAJ 変数アクションスクリプトジャンプ (Value Action Script Jump)
VLAJ=n, v, EqlAcsID, NeqAcsID
n: 変数番号(0〜)
v: 比較値
EqlAcsID: 一致時のジャンプ先(0は無視)
NeqAcsID: 不一致(0は無視)
PCRJ 姿勢状態 (Posture Condtion Case Branch by Record)
PCRJ=d, Dir0, Dir1
d: チェック方向 LGW:前後、SID:左右、ALL:全判定
Dir0: 前、または左検出時のジャンプ先レコード
Dir1: 後、または右検出時のジャンプ先レコード
※ALL時に倒れ判定が出た場合、Dir0のジャンプ先に移動
DSRJ 距離センサ (Distance Sensor)
DSRJ=l, d, GreaterJump, LowerJump
l: モード(BTO/LFT/RGT/BTA)
BTO どちらかが検査値以下ならFALSE(BOTH OR)
LFT 左センサ評価
RGT 右センサ評価
BTA 両方が検査値以下ならFLASE(BOTH AND)
d: 検査距離(0〜63)実際には遠いほど数値が小さい。また線形変化ではない。
GrJump: 検査距離以上の場合のジャンプ先レコード
LwJump: 検査距離未満の場合のジャンプ先レコード
PLRJ 電源電圧 (Power Level Record Jump)
PLRJ=n, SmallerJump, GreaterJump
n: 電源電圧*10
SmallerJump: しきい値未満レコードジャンプ(0の時は次のレコードへ)
GreaterJump: しきい値以上レコードジャンプ(0の時は次のレコードへ)
SCDG サーボ角度指定 (Servo Control by Degree)
SCDG=s, m, m, Clock, Degree
s: サーボ番号
m: 動作モード(SFT/LNR/HRD)
SCXZ サーボ座標指定 (Servo Control by XZ)
SCXZ=LegMode, m, m, Clock, X, Z
l: 脚モード(LFT/RGT)
m: 動作モード(SFT/LNR/HRD)
Clock: 動作クロック
X: X座標(mm)
Y: Y座標(mm)
Back to G-Tune 2004FI
Index