目次 前 次

移動メカ制御(1)

 
　移動メカを用意して、動かしてみます。
　使う移動メカは、以下。



　この移動メカには、サーボモータとDCモータが
　各１個ずつ実装されています。

　IchigoJamのBASICで使われているPWM命令を
　タイマー０、１に適用してみます。

　PWM命令は、３パラメータを持っています。

　ワードで接続先を固定すれば、ポート番号は
　不要と判断できます。

　サーボモータを利用するときは、100kHzをカウンタの
　クロックとして最大カウント値を2000、カウント値は
　100から200にして対応。

　タイマーのクロックを100kHzに設定して、コンペア
　マッチで扱う１周期を2000にします。
　これで、サーボモータは100から200のいずれかの
　整数値を指定するだけになります。

　PWM波形をどこから出力するのかを考えて
　ピンアサインで、OUT_2、OUT_3に相当する
　ポートを使うことに。




　OUT_2、OUT_3は、PIO1_1、PIO1_2を使います。
　タイマーは、CT32B1で32ビットカウンタ利用に。

　コンペアマッチを使うので、プリスケーラと
　カウンタの設定値を考えていきます。

　システムクロックをデフォルトの12MHzで動作させると
　すれば、プリスケーラは120分周で100kHzをカウンタに
　与えることができます。プリスケーラには、119を設定。

　CT32B1を利用するので、IOCONFIGレジスタでPIO1_1、PIO1_2は
　コンペアマッチ出力に設定しなければなりません。

　また、SYSAHBCLKCTRLレジスタのパラメータ設定で、CT32B1に
　クロック供給すると指定。

　サーボモータ、DCモータともに、コンペアマッチで値が一致
　したときから、’L'を出力するようにします。正論理で利用
　する仕様で対応。

　ドライブ回路は、以下。




　サーボモータは、電圧レベルが問題なので、LPC1114のピンから
　出力される電圧をサーボモータの電源電圧レベル近くまで引き
　あげておきます。DCモータは、電力を与えるようにして対応。

　初期化ワードは、以下。

\ SFR
$4004407C constant PIO1_1
$40044080 constant PIO1_2
$40048080 constant CLKCTRL
$40018000 constant CT32B1IR
$40018004 constant CT32B1TCR
$40018008 constant CT32B1TC
$4001800C constant CT32B1PR
$40018010 constant CT32B1PC
$40018014 constant CT32B1MCR
$40018018 constant CT32B1MR0
$4001801C constant CT32B1MR1
$40018020 constant CT32B1MR2
$40018024 constant CT32B1MR3
$4001803C constant CT32B1EMR
$40018074 constant CT32B1PWMC

: init_motor ( -- )
  \ set P1.1 compare match output with CT32B1_MAT0
  3 PIO1_1 !

  \ set P1.2 compare match output with CT32B1_MAT1
  3 PIO1_2 !

  \ enable CT32B1
  1 10 lshift CLKCTRL bis! 

  2 CT32B1TCR ! \ disable Time Control Register

  1 7 lshift CT32B1MCR bis! \ MR2 -> Reset TC

  119 CT32B1PR !  \  Set Prescale Register

  150 CT32B1MR0 ! \ 150 -> MR0

  0 CT32B1MR1 ! \ 0 -> MR1

  1999 CT32B1MR2 ! \ 1999 -> MR2

  3 CT32B1EMR bis! \ refrects MR0 and MR1 to MAT_0 and MAT_1

  5 4 lshift CT32B1EMR bis! \ if compare matched , clear output

  0 CT32B1PWMC ! \ disable PWM controller

  1 CT32B1TCR ! \ enable Time Control Register
;

　サーボモータに与えるパラメータを
　-90から+90にして、わかりやくする
　ワードを定義。

: svm
  5 * 9 / 150 + CT32B1MR0 !
;

　定義域の-90から+90を、カウント値の100から200と
　するために、次の計算式を使います。



　動作試験には、次のワードを定義して対応。

: xdelay 1000000 0 do i loop ;

: tst_svm 91 -90 do i dup . svm xdelay loop ;

　「i dup .」とするのは、サーボモータの制御
　パラメータを与えるのと同時に角度を表示して
　わかりやすくするため。

　DCモータに与えるパラメータをDUTY比として
　処理するワードを定義。

: dcm
  20 * CT32B1MR1 !
;

　動作試験には、次のワードを定義して対応。

: tst_dcm 100 0 do i dup . dcm xdelay loop ;

　「i dup .」とするのは、DCモータのDuty比を
　パラメータで与えるのと同時に値表示により
　わかりやすくするため。

　モータを動かすときには、次のように
　ワードを入力します。

\ initialize
init_motor{enter}

\ test servo motor
tst_svm{enter}

\ neutral
150 svm{enter}

\ test DC motor
tst_dcm{enter}

\ stop DC motor
0 dcm{enter}
目次 前 次

inserted by FC2 system