クモ、手足を得る (2)実践編
STATUS: 故障中x2
USBSPYDER08からラジコンサーボの制御が出来るようになりました。
(参考→準備編)
サーボ制御の信号を簡単に説明すると、15ms〜20msの一定周期で、パルスを送り、そのパルスの長さ(1.5ms〜2ms程度)がサーボの角度となります。
周期やパルスの長さは制御するサーボによって仕様がことなるようですが、だいたいこんなかんじのようです。
波形的にみると、ONとOFFを繰り繰り返す矩形波となります(デジタル的なONとOFFだけの波形)。
このONとOFFの比率をデューティ比といい、たとえば20ms周期の波形で1msがONの場合の、デューティ比は、1:20 あるいは 5%となります。
デューティ比によって、信号を表す方式をPWM(Pulse Width Modulationの略)といい、上で説明したサーボ角度の信号も、PWM信号ということになります。
USBSPYDER08のCPUである、MC9S08QG4/8の「タイマ/ パルス幅モジュレータ(S08TPMV2)」を利用すると、設定した周期・ディーティ比のPWM信号を生成し、それをそのままI/Oのピンに出力できます。
ここのサーボを直接繋げばOKです。
回路は以下の図のようになります。
受信機とサーボの間に割り込んで、制御信号をサーボに送り込みます。
受信機からの信号線は切断しているので、実は受信機は不要で5Vの電源で代用出来ます。
マイコンには、10KΩボリュームを繋ぎます。
ボリュームをまわすと、それにしたがってサーボの角度が変わるようにプログラムします。
電源(5V) ││ ─┬─ ┌┴┴──┐ ┌─┴─┐ │受信機 ├─ 5V(赤)────────┤サーボ│ │ ├─ GND(黒) ────●──┤ │ │ ├─ 信号(白) ─× │┌─┤ │ │ │ ││ └───┘ │ │ ┌─────┘│ └────┘ │ │ │ │ +3V ─────●─┼┐ │ GND ─●───┼─●│││ │ │ │┌┴┴┴┴┐ │ │ ││4 3 2 1 │ │ │ ││マイコン│ │ │ ││ 5 6 7 8│ │ │ 10KΩ│└┬┬┬┬┘ │ └vvvvvv┘ ││││ │ ↑ │ │ │ └───┘ └───┘
コードは以下のとおり。
#include <hidef.h> /* for EnableInterrupts macro */ #include "derivative.h" /* include peripheral declarations */ // ICSTRM location in Flash unsigned char ICSTRM_FLASH @0xFFAF; int adc_val=0; void main(void) { int n1,n2,i; EnableInterrupts; /* enable interrupts */ // DIO PTADD = 0b00000001; // PTAD bit 0 = out // タイマ/ パルス幅モジュレータ設定 // 7 TOF : (ReadOnly) // 6 TOIE = 1 : Interrupt Enable // 5 CPWMS = 0 : // 43 CLKS = 01 : Clock Source = BUSCLK // 210 PS = 010 : PreScale = 1/4 TPMSC = 0b01001010; TPMMOD = 20000; // Timer Counter Modulo ( 20ms ) // 7 CHnF = 0 // 6 CHnIE = 0 : Interrupt Enable = off // 5 MSnB // 4 MSnA // 3 ELSnB // 2 ELSnA TPMC0SC = 0b00101000; TPMC0V = 1000; // パルス幅 (後で書き換える) // A/Dコンバーター設定 // ADC start // 7 COCO // 6 AIEN // 5 ADCO // 43210 ADCH (select AD03 = 00011) ADCSC1 = 0b00000011; // メインループ for(;;) { if ( ADCSC1 & 0b10000000 ) { TPMC0V = 20000/10/255*ADCRL; // from ADC 8bit value (0-255) ADCSC1 = 0b00000011; // ADC restart } __RESET_WATCHDOG(); /* feeds the dog */ } /* loop forever */ /* please make sure that you never leave main */ } interrupt 7 void Timer1Overflow (void) { TPMSC_TOF=0; }
「タイマ/ パルス幅モジュレータ設定」の部分が今回の肝です。
単純なタイマーとしては、以前使いましたが、今回はサブのカウンター(TPMC0SC)も使用し、PWMを出力します。
TPMSCの設定では、クロックソースとしてバスクロック(4Mhz)を選択し、プリスケーラで1/4にしています。このため、タイマーの内部カウンターは、1/1000000秒ごとにカウントアップすることになります。
TPMMODでモジュラ値をして20000を指定していまうので、タイマーの周期は、20000/1000000秒 = 20/1000秒 = 20ミリ秒周期となります。
割り込みを有効にしていますので、この周期で、Timer1Overflowが呼ばれます。が、実はここではなにもしていませんので、割り込み向こうでもいいです。
今回は新たに、TPMCnSCの設定も行っています。
「タイマ・チャネルn ステータス/ 制御レジスタ」です。2チャンネルあるので、nには、0か1となります。今回はチャンネル0を使うので、TPMC0SCです。
ここでの設定は、エッジアラインPWMで、HIGHの時に信号ピン(PTAD0)がONになる設定としました。(MSnB:MSnA = 10 , ELSnB:ELSnA = 10)
大抵はこの設定でしか使わないんじゃないかなあ。
(値の意味はデータシートの213ページ参照)
TPMC0Vは、PWMでONになっている時間を指定します。
マインのタイマーのTPMMODに20000を指定していますので、たとえばTPMC0Vに半分の値である10000を指定すると、10ミリ秒ONの後10ミリ秒OFFになる、デューティ比1:0のPWM信号になります。
ONの時間を1ミリ秒にしたいときは、1000を指定すればよいです。
これで、PTAD0のピンにはPWMでパルスが出力しはじめました。
TPMC0Vの値を変えれば、サーボの角度がかわります。
サーボの制御、出来ちゃいました。
メインループでは、ADCでボリュームの値(ADCRL)を取得し、TPMC0Vに書き込みことで、ボリュームの角度をもとにサーボの角度を設定しています。
TPMC0V = 20000/10/255*ADCRL;
ボリュームからの値(0〜255)によって、TPMC0Vが、0〜2000(パルス幅:0ミリ秒〜2ミリ秒)の値になります。
ちなみにサーボの仕様では、パルスの幅は、1.5ミリ秒〜1.9ミリ秒でサーボが左右に振り切れることになるので、仕様を越えた信号送るとサーボがかわいそうです。ほどほどにしましょう(ぉ
ちなみに上のように作ったところ、サーボがいつまでも振動してピタっと止まりませんでした。
これには悩みましたが、PWMの信号が安定しないのかなーとか。でも、原因は単純なところにありました。
ADCでボリュームの値を読み込んでいるのですが、これが一定の値をしめさず、常に10%程度の範囲で変動していました。
ボリュームからマイコンへの配線は手抜きしてICクリップつきのケーブルでつないだだけだったので、この配線へのノイズなどが原因で入力値が安定しなかったのでした。アナログ部分は手を抜くとすぐ問題化しますね・・・反省
入力値が一定値以上変化したときのみ反映するように改良したらサーボの動きは安定しました。
この問題解決にあたっては、witchさんにはいろいろアドバイスいただきありがとうございました。
サーボについては二足歩行ロボット関連でノウハウが沢山あると思いますので参考にさせていただきます。