知能ロボットコンテスト’９９ ロボット製作資料Ｎｏ１ – R.U.R.
知能ロボットコンテスト’９９ ロボット製作資料
作成 ｄｅ ＪＭ４ＬＦＡ ９９．５．２７
FOR628.C または T98.C 等のソースファイルで使用されている関数について簡単な解説
関数解説
| ａｒｍ（） | アーム制御関数です。引数は、ARM_UP,ARM_DOWN,OPEN,CLOSE,STOP_ARM
の５つです。アームはハードウエアでリミットスイッチを用いた、ドライブ回路なので、ソフトウエアで、地面に接地したかどうか判断できませんので、十分に時間を取って待ちましょう。 |
| ｗａｉｔ（） | ｕｎｓｉｇｎｅｄ ｉｎｔ型を用いたソフトウエアループによるウエイト関数です。よって引数は、正の数で、最大６５５３５です。 |
| ｐｏｋｅＶ２５ｒｅｇ（） | 割り込み関数をつかうことを許可するときに使用します。ロボットスタート時に呼び出します。 |
| ｐｐｉ＿ｉｎｉｔ（） | ８２５５を初期化します。プログラムの最初あたりにこの関数を呼び出すだけでＯＫです。引数はなし。 |
| ｔｉｍｅｒ＿ｉｎｉｔ（） | これは割り込みのサンプリングレートを変えたいとき意外は、しらなくていい関数です。スタート時あたりに引数なしで呼び出すだけでＯＫです。なにも変更しなければ、１秒間に２０回、ｓａｍｐｌｉｎｇ（）関数が呼び出され、位置計算等が行われます。 |
| ｓａｍｐｌｉｎｇ（） | ソフトウエア割り込みによって、１秒間に２０回呼び出される関数です。標準の機能は、ロータリーエンコーダーを読みとり、その値を用いて、ｉｔｉ（）関数により、現在位置を計算させます。位置計算された結果は、グローバル変数にセットされます。そこでプログラマは、その値をもとに、目標速度を表す、グローバル変数に目標速度（１／２０秒ごとのロータリーエンコーダーのカウント値）をセットさせる関数を自作し、このｓａｍｐｌｉｎｇ（）関数から呼び出してください。その後、ｓｐｅｅｄ＿ｃｏｎｔｒｏｌ（）関数によって、目標速度に追従するＰＩ制御計算され、モーターへの操作量がグローバル変数にセットされ、ｍｏｔｏｒ（）関数によって、モータがＰＷＭ速度制御され、ロボットが動きます。詳しい説明は、後述してます。 |
| ｓｐｅｅｄ＿ｃｏｎｔｒｏｌ（） | 引数は、そのときのロータリーエンコーダーのカウント値です。ＰＩ速度制御によりモーター操作量を目標速度（ｉＴａｒｇｅｔＶｅｌｏ＿ｒ、ｉＴａｒｇｅｔＶｅｌｏ＿ｌ）をもとに計算し、グローバル変数 ｉＭｏｔｏｒＯｕｔ＿ｒ、ｉＭｏｔｏｒＯｕｔ＿ｌにセットします。とくにこの関数については、変更の必要はないと思います。 |
| ｍｏｔｏｒ（） | 引数はｉｎｔ型で、－４１６から＋４１６までです。マイナスならモータは逆回転します。ＰＷＭ速度制御です。第１引数が左モータ、第２が右です。通常は、ＰＩ制御計算された操作量をしようするので、ｍｏｔｏｒ（ｉＭｏｔｏｒＯｕｔ＿ｌ，ｉＭｏｔｏｒＯｕｔ＿ｒ）；のままでＯＫです。 |
| ｉｔｉ（） | 現在位置計算関数です。引数は、ロータリーエンコーダーカウント値です。計算結果は、グローバル変数 ｘ，ｙ，ａｒｇ に代入されます。座標系は、スタート時のロボット位置のロータリーエンコーダー軸の中心点が（０，０）で、角度は、最初が０ラジアンです。詳しいことは現在不明。後ほど報告します。 |
なにをかんがえればいいか？
ロボットは２０分の１秒ごとの現在位置、向き（多少の誤差あり）がわかっているので、それをもとに左右のモーターの目標速度を設定するだけです。（この場合、その変数に数値を代入するだけで、勝手にＰＩ制御関数が追従してくれる）
具体的には、ある目標地点にたいして、どのようにして近づくか？です。
たとえば、目標地点と現在の位置がずれていて、まず、ロボットの角度を修正して、あとは、直進とか。（この場合、近付くにつれ、角度の誤差が拡大されるし、近付けば近付くほど、角度の修正は難しくなります。）
参考までに昨年の方法をかいておくと、ロボットがほぼ直線で数メートルすすめることを仮定して、目標地点との距離を√（ｘ＊ｘ ＋ ｙ＊ｙ）でなくて、この場合なら ｙ として、（ｘ方向にある範囲からはみ出なければ、）、ｙの値を係数倍したものを左右のモーターの目標速度として、与えています。制御の言葉で言うと比例制御、Ｐ制御というのかな？ で、横方向は無視できないほどはみでちゃった場合は、はみでてすぐなら、１ｃｍくらいしかはみでてないので、はみでたほうのモーターを多めにまわすとか。（ここは、どうしてたか覚えてないです）
がんばれーーーー！！ （＾ ＾）／
知能ロボットコンテスト９８ロボット「つみたて君」のプログラムの
フローチャートみたいな。
まずは、ｍａｉｎ（）関数から追っかけていくとしよう。（割り込み起動関数は後回し）
- 各種関数初期化
- アームを持ち上げて。（あげるまでの間しばらく待つ）
- 後ろのドアを閉じて。（しまるまでちょっとまつ）
- ＰＩ制御の係数を初期化
- ホストコンピュータの画面を改行しまくって、きれいにして、Ｓｔａｒｔ ＯＫ と表示
- コンピュータＢｏｘの蓋にある、ロータリースイッチが１になるまで待つ（０以外になるとロボットスタート）
- ロボットが完全に静止するまで待つ（人間によって動かされている間はとまっとけ ということ）
- 割り込みを許可する
- 無限ループ（この後にも、かいてあるけど、たぶん実行されることはない）
ｍａｉｎ（）関数で割り込みを許可したけど、どうなるの？
うーん、それはね、ｍａｉｎ（）で初期化した関数で ｔｉｍｅｒ＿ｉｎｉｔ（） って関数があるでしょ。それが、２０分の１（変更可能だよ）秒ごとに、 ｓａｍｐｌｉｎｇ（） って関数を呼び出す仕組みになってるんだ。
じゃあ、以下に ｓａｍｐｌｉｎｇ（） 関数の流れ図（？）を示そう。４～８は重要なので、表示を変えてあります。
- ロータリーエンコーダーの値を保存する変数を宣言
- ロータリーエンコーダーのカウント値を読みとって、変数に代入
- そのカウント値をもとに、現在位置を計算する。 ｉｔｉ（） 関数がそれ。現在位置は、グローバル変数に保存されてるよん。
| ４． | 行動命令実行可能状態 ｐｒｏｃｅｓｓ==0 なら、（ちなみに命令実行中ならｐｒｏｃｅｓｓ==1だ） 次のロボット行動命令を読み込む（ａｄｍｉｎｉｓｔ（）関数） 以下の５から８は読み込んだ命令を実行、処理する |
| ５． | その場回転しろ という命令だったならば 回転する |
| ６． | 「止まれ」命令なら ロボットが完全に静止するまで待つ（正確には、瞬間速度が、０であればＯＫってこと） |
| ７． | 「アームを動かせ」命令実行中なら、 ｔｉｍｅ＿ｃｏｕｎｔ変数が２００になるまで アームを動かす |
| ８． | 上記の３つの条件分岐（５，６，７）のどれにも当てはまらなかった場合 ロボットの位置が目標位置となるよう、position_control() 関数を呼び出す。ＥＮＤ＿ＰＨＡＳＥとなったら（目標位置に到達したら） ｐｒｏｃｅｓｓ変数を命令受け入れ可能状態の０を代入、ｐｈａｓｅを１つすすめる。 |
- ９．ＰＩ制御スピードコントロール関数 ｓｐｅｅｄ＿ｃｏｎｔｒｏｌ（） を呼び出し、目標速度（グローバル変数）と、引数の現在の速度（エンコーダー読みとり値）から、モーター入力値を求める。
- １０．計算された入力値をもとに、モータを駆動する。
- １１．割り込み関数終了関数 ｆｉｎｔ（） を呼び出して、割り込み関数を終了させる。
肝心の位置（ｘ、ｙ）の制御は？
寝る。またこんど。できれば、つくりなおしてほしいし。
回転の制御は？ ｒｏｔａｔｉｏｎ＿ｃｏｎｔｒｏｌ（） その場回転だ。
- 変数宣言だ。
- 目標角度が１８０度方向か、それ以外で分岐
目標角度が１８０度の場合
（特別扱いが必要なため）目標角度を代入する。
目標角度と現在の角度の差を取る（偏差）
それ以外の場合 目標角度を代入
偏差を取る
３．偏差が許容値以内なら、ロボットを停止させ、回転制御関数を抜ける。（戻り値は ＥＮＤ＿ＰＨＡＳＥ）
４．偏差の５０倍を操作量とする。
５．回転最高速を越えていたら、設定最高速にする。
６．操作量を左右のモーターの目標速度に代入。
アーム操作関数 ａｒｍ（） は？
アームをそのときの引数に従って、動かす信号をだすだけ。ハードウエアでリミットスイッチをもっているので、アームが停止位置にきたら、勝手に、止まってくれます。 ただ、それだけ。
Do not copy without permission.
