移動メカ制御(3)
移動メカを動かすためには、センサーからの情報 取得が必要です。 LPC1114のポート０の２から５に路面センサーから センサー情報を取得するとしておきます。 ピンアサインは、以下のようになっています。ポート０の２から５をGPIOで利用するためには 次のスクリプトを与えればよいでしょう。 \ SFR $4004401C constant PIO0_2 $4004402C constant PIO0_3 $40044030 constant PIO0_4 $40044034 constant PIO0_5 \ Set as GPIO with Pullup 8 PIO0_2 ! 8 PIO0_3 ! 0 PIO0_4 ! 0 PIO0_5 ! GPIOで利用することを指定したので 方向設定レジスタで、４ビット分を 入力に設定。 \ SFR $50008000 constant GPIO0_DDR \ Set P0.2 - P0.5 as Input $FC3 GPIO0_DDR ! LPC1114は、メモリマップドI/O方式を採用して いるので、レジスタを使って外部から情報入力 すればよいでしょう。 \ SFR $50003FFC constant GPIO0_DR \ get nibble and show GPIO0_DR @ 2 rshift hex . LPC1114では、ビットごとの入出力を単純にする 目的で、次のようにアドレスを振って処理します。
PIOの値とアドレス$50003FFCの$3FFCとの 論理積をもとめて、レジスタに保存します。 PIO0_2からPIO0_5の２ビットを右論理シフト で整えてから表示。 スタックに２ビットがあるので、変数に入れて おけば、必要なときに取り出して利用可能。 現在のPIO0_2からPIO0_5の設定状態を確認。
全ピンがGPIOに設定されているとわかったので 入力指定しておきます。
利用するピンに、プッシュスイッチを接続。
この状態で、入力値を表示してみます。
２ラインずつ見ていきます。 上の２ラインは、プッシュボタンを押していない、押している ときの値になります。 下の２ラインもプッシュボタンを押していない、押している ですが、右シフトして４ビットの並びが判読しやすいように してます。 より使いやすくするために、次のワードを定義。 : get_sensor GPIO0_DR @ 2 rshift $00F and . ; このワードで、プッシュスイッチを押しながら、どんな 値が表示されるのかを実験してみました。
これで、４ビットの値を入力することができるように なったので、利用ワードとしてまとめておきます。 \ SFR $50008000 constant GPIO0_DDR $50003FFC constant GPIO0_DR 0 variable SENSOR \ init_io Set P0.2 - P0.5 as Input : init_io $FC3 GPIO0_DDR ! ; \ get sensor : get_sensor GPIO0_DR @ 2 rshift $00F and SENDOR ! ; 変数SENDORに情報を入れておけば、必要な ときに利用できるとなります。 移動メカを動かすときに、トグルスイッチを使えば スタートとストップが簡単になるので、スイッチの 状態を使い、全体の動作を制御できます。 PIO0_6に、トグルスイッチを接続します。
PIO0_6に、論理値の'H'が印加されるまで、待つことが できれば、スタートボタンを入れるまで、動き出さない ようにできます。 \ SFR $50008000 constant GPIO0_DDR $50003FFC constant GPIO0_DR \ init_io Set P0.2 - P0.6 as Input : init_io $F83 GPIO0_DDR ! \ Wait : xwait begin GPIO0_DR @ 5 rshift 1 and until ; 移動メカを動かすときのシーケンスを考えると以下。
必要なワードが定義済みとすれば、次のコードで マシンを動かすことができます。 : stop_motor 0 svm 0 dcm ; : mcr \ initialize init_io init_motor \ stop stop_motor \ wait start trigger xwait \ endless loop begin GPIO0_DR @ 5 rshift 1 and while \ get road state get_sensor \ ALL_BLACK SENSOR @ 0 = if 0 svm 10 dcm then \ ALL_WHITE SENSOR @ 1 = if 0 svm 20 dcm then \ LEFT_WHITE SENSOR @ 2 = if 0 svm 25 dcm then \ RIGHT_WHITE SENSOR @ 3 = if 0 svm 25 dcm then \ CENTER SENSOR @ 4 = if 0 svm 50 dcm then \ TINY_RIGHT SENSOR @ 5 = if -5 svm 45 dcm then \ RIGHT SENSOR @ 6 = if -10 svm 35 dcm then \ BIG_RIGHT SENSOR @ 7 = if -15 svm 25 dcm then \ TINY_LEFT SENSOR @ 8 = if 5 svm 45 dcm then \ LEFT SENSOR @ 9 = if 10 svm 35 dcm then \ BIG_LEFT SENSOR @ 10 = if 15 svm 25 dcm then \ BOTH_WHITE 11 SENSOR @ 11 = if 0 svm 5 dcm then \ delay xdelay repeat \ stop stop_motor ; またワード case を利用しても記述可能。 : stop_motor 0 svm 0 dcm ; : mcrx \ initialize init_io init_motor \ stop stop_motor \ wait start trigger xwait \ endless loop begin GPIO0_DR @ 5 rshift 1 and while \ get road state get_sensor \ store value into stack SENSOR @ \ judge case \ ALL_BLACK 0 of 0 svm 10 dcm endof \ ALL_WHITE 1 of 0 svm 20 dcm endof \ LEFT_WHITE 2 of 0 svm 25 dcm endof \ RIGHT_WHITE 3 of 0 svm 25 dcm endof \ CENTER 4 of 0 svm 50 dcm endof \ TINY_RIGHT 5 of -5 svm 45 dcm endof \ RIGHT 6 of -10 svm 35 dcm endof \ BIG_RIGHT 7 of -15 svm 25 dcm endof \ TINY_LEFT 8 of 5 svm 45 dcm endof \ LEFT 9 of 10 svm 35 dcm endof \ BIG_LEFT 10 of 15 svm 25 dcm endof \ BOTH_WHITE 11 11 of 0 svm 5 dcm endof endcase \ delay xdelay repeat \ stop stop_motor ; このワードで、次のコースを走行。
- 初期化
- スタートボタンが入れられるまで待機
- スタートボタンが入っていれば４に進み、そうでなければ８に進む
- センサーから情報入手
- センサー情報を使い、モータにパラメータ設定
- 時間待ち
- ３に戻る
- モータを停止
ワィンディングロードだけを完走できることが できたなら、クランク走行、レーンチェンジに 進んでいけばよいと思っています。
目次 前 次