自作引き込み脚コントローラー

PICマイコンでTAIYO零戦52型の引き込み脚サーボを動かすコントローラーを作ってみました。

普通のサーボはポテンションメーターが出力軸に入っていて位置決めフィードバック制御をしているのですが、この純正の引き込み脚サーボは単にモーターが入っていて、180度回った位置でメカ的に(サーボの箱の中にある)ストッパーにあたって止まるだけのようです。最初に乾電池2個で、直接回してみました。+-を逆にすると、逆に回って止まります。
と言うわけで、
プロポのGEARスイッチ上で+-方向に電圧を掛けて、適当な時間でOFFする。
プロポのGEARスイッチ下で-+方向に電圧を掛けて、適当な時間でOFFする。
と言うことにしました。(適当な時間は現在5秒にしています。なので負荷が掛かって動きが渋いと途中で止まります)
これだけなのですが、プログラム作成は結構たいへんでした。何しろ、アッセンブラーですから。

この写真は最初に作った物。動き的にはOKなのですが、トランジスター4つでHブリッジを作りモーターの正転、逆転をしているので、ゲート駆動用に抵抗が4本必要なのと、トランジスタの損失なのか何か分かりませんが、電源電圧5Vに対して、モーターに掛かる電圧が3Vぐらいしか出ませんでした。3Vでは、いい感じにゆっくり動くのですが、ちょっとパワー不足で途中で止まったりしてしまいました。(左のICソケットにPICが入ります。)
b0121633_203951.jpg


純正の足が折れてしまったのを機に、GWSのMM/XLに交換し、リンケージもきれいにやり直しました。それに合わせて、トランジスタのHブリッジをやめ、専用のDCモーター駆動用ICを採用しました。
その写真がこれ。部品はたったの2つです。PICは12F683、モータードライバーはTA7291Sです。これでモーターへの電圧が4.8Vぐらいになり、かなり動きが軽快になり、確実に脚にロックが掛かるようになりました。
b0121633_20445284.jpg


以下はプログラムのソースコードです。個人で使われる場合はご自由にどうぞ。あまりいいプログラムでは無いですが。使用される場合はあくまで自己責任で。

LIST P=P12F683
INCLUDE "P12F683.INC"
__CONFIG _CP_OFF & _CPD_OFF & _WDT_OFF & _BOD_ON & _IESO_OFF & _PWRTE_ON & _INTOSCIO & _MCLRE_OFF


ORG 0


MOVLWB'111'
MOVWFCMCON0;GP2-0はデジタルIO

BSFSTATUS,RP0
MOVLW B'001000';GP5のみ入力
MOVWFTRISIO
MOVLW070h; CLOCK=8MHz
MOVWFOSCCON
CLRFANSEL;デジタルIO(AN機能を使用しない)
BCFSTATUS,RP0

PLSEQU20H
BLIMEQU21H
SLIMEQU22H
FLAGEQU23H

BSFGPIO,5;CLRFGPIO
BSFGPIO,4
BCF GPIO,1
BCFGPIO,2

CLRFPLS
CLRFBLIM
CLRFSLIM
CLRFFLAG


MAIN
BTFSC GPIO,3;0ならスキップ 1なら実行
GOTO P0

BTFSS FLAG,0
GOTO MAIN
BCF FLAG,0

MOVLW 8EH
SUBWF PLS,W;キャリーが1でPLSの方が大きい、0で小さい
BTFSS STATUS,C;0なら実行
GOTO P1
MOVLW 0EEH
SUBWF PLS,W;マイナスでキャリー0
BTFSS STATUS,C;
CALL BIG
GOTO MAIN
P1
MOVLW 2FH
SUBWF PLS,W
BTFSC STATUS,C
CALL SMALL
GOTO MAIN

P0
BTFSS FLAG,0;0なら実行
CALL P2;OFFならP2
INCF PLS,1
BTFSC STATUS,Z
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
GOTO MAIN
P2
CLRF PLS
BSF FLAG,0
RETURN

;大の処理************************************************
BIG
BTFSS FLAG,1
CALL BIG2
BTFSC GPIO,2
INCF BLIM,1
MOVLW 96H
SUBWF BLIM,W;キャリー1でプラス
BTFSC STATUS,C
CALL BIG3
RETURN

BIG2
CALL SMALL_OFF;BCF GPIO,5
CLRF SLIM
CALL BIG_ON;BSF GPIO,4
BSF FLAG,1
BCF FLAG,2
RETURN

BIG3
CALL BIG_OFF;BCF GPIO,4
CLRF BLIM
RETURN

;小の処理*************************************************
SMALL
BTFSS FLAG,2
CALL SMALL2
BTFSC GPIO,1;1なら実行
INCF SLIM,1
MOVLW 96H
SUBWF SLIM,W
BTFSC STATUS,C
CALL SMALL3
RETURN

SMALL2
CALL BIG_OFF;BCF GPIO,4
CLRF BLIM
CALL SMALL_ON;BSF GPIO,5
BSF FLAG,2
BCF FLAG,1
RETURN

SMALL3
CALL SMALL_OFF;BCF GPIO,5
CLRF SLIM
RETURN

BIG_ON
BCFGPIO,4
BSFGPIO,2
RETURN


BIG_OFF
BSF GPIO,4
BCF GPIO,2
RETURN


SMALL_ON
BCF GPIO,5
BSF GPIO,1
RETURN


SMALL_OFF
BSF GPIO,5
BCF GPIO,1
RETURN




END
[PR]
by itsumo-sunday | 2008-03-13 20:48 | TAIYO 零戦52型
<< 初離着水成功 無惨な姿に >>