2018年1/23の上記の記事では、OnOff制御のサーボアンプソフトを掲示していたが、
ニュートラルなどがいまいち定まらず不満に思っていたので、空用プロポプロジェクト
終了の機会にモーター駆動をOnOff制御ではなく、信号と角度情報の差分に応じた駆動
パルス比例制御に変更改造した。多分これが市販のデジタルサーボと同じ方式のはず。
プログラムは以下の通り。 これで、満足の行くラダーサーボになった。
; 雪風のサーボｱﾝﾌﾟソフトその6
R6(2024)7/11 改良を行う 作動OK!! アンプの完成！！
サーボパルスの幅Tを測定後、ポテンショの電圧をAD変換でPを求め、(T-P)が-の時正回転、
(P-T)が-の時逆回転、P=Tでは停止の信号となる４個のTrスイッチの組み合わせを設定する。
PとTは0～255の整数で、(P-T+2)*1mSecの値に応じた時間+5Vのモーター駆動パルスを発する。
ただし、｜P-T｜＜＝1の不感幅、P-T+2の加速加算と14ｍSecの駆動パルス制限を設ける。
; 初期設定 ----------- 152 Line ----------
LIST P=PIC12F675
INCLUDE "P12F675.INC"
__CONFIG _INTRC_OSC_NOCLKOUT & _PWRTE_ON & _CPD_OFF & _CP_OFF & _BODEN_ON & _MCLRE_OFF & _WDT_OFF
COUNT1 EQU 20h
COUNT2 EQU 21h
COUNT3 EQU 22h
TWID EQU 23h
POTN EQU 24h
ORG 0
BSF STATUS,RP0 ;Bank1
CALL 3FFh
MOVWF OSCCAL ;4MHz Internal Clock Calibration
MOVLW b'00001001'
MOVWF TRISIO ;GP0,3=Input GP1,2,4,5=Degital-Output
MOVLW b'01010001' ;GP0=Analog Input ポテンショ電圧測定用 AD変換はFOSC/16：Tad=4μSec
MOVWF ANSEL ;GP3=Degital Input 受信パルス解読用
CLRF VRCON ;Comp off
BCF STATUS,RP0 ;Bank0
MOVLW b'00000111' ;Comp off
MOVWF CMCON
BCF GPIO,1
BSF GPIO,2
BSF GPIO,4
BCF GPIO,5
;---------------------------
; Main Loop (15~20mSec周期)
;---------------------------
MAIN
CALL SIGNALANAL ;パルス幅測定して TWID をセット TWID=100～150～200 (1~2 mSec Spend)
CALL POTENTIO ;ポテンショ電圧測定し POTN をセット POTN=0～255 (44 μSec Spend)
; 比較 (TWID-POTN)と(TWID-POTN)から モーター回転判断
CHK1 MOVF POTN,0 ; まずTとPの大小関係を調べる
SUBWF TWID,0 ; W=T-P
BTFSC STATUS,C ; C=0すなわち上記引き算がマイナスの時(T<P)次スキップ
GOTO CHK2 ; T>=P へ
GOTO RITN ; T<P 右回転
CHK2 MOVF TWID,0 ; T>=P の場合
SUBWF POTN,0 ;W=P-T
BTFSC STATUS,C ; C=0すなわち上記引き算がマイナスの時(T>P)、次スキップし左回転
GOTO MSTOP ; P=T
GOTO LFTN
; モーター制御
MSTOP BCF GPIO,1 ; T=Pの場合 および駆動パルスの停止
BSF GPIO,2
BSF GPIO,4
BCF GPIO,5
CALL WAIT1mS
GOTO MAIN
LFTN MOVF POTN,0 ; T>Pの場合 左回転
SUBWF TWID,0 ; W=T-P
SUBLW d'1' ; W=1-(T-P) |T-P|=1 の不感帯を設ける
BTFSC STATUS,C ; C=0すなわち上記引き算がマイナスの時(T-P)>1、次スキップし左回転
GOTO MSTOP
MOVF POTN,0
SUBWF TWID,1 ; TWID=T-P
MOVLW d'2' ; 加速加算 3以上ではオーバーシュート気味になる
ADDWF TWID ; TWID=T-P+2
MOVLW d'14'
SUBWF TWID,0 ; W=T-P+2-14
BTFSS STATUS,C ; C=1すなわち上記引き算が>=0の時(T-P+2)>=14、次スキップ
GOTO LTN1
MOVLW d'14'
MOVWF TWID ; TWID=14 最大14ｍSec制限
LTN1 BSF GPIO,1
BSF GPIO,2
BCF GPIO,4
BCF GPIO,5
LTN CALL WAIT1mS ; (T-P+2)*1mSec (最大14ｍSec制限)の+5Vモーター駆動パルスを生成
DECFSZ TWID,1
GOTO LTN
GOTO MSTOP
RITN MOVF TWID,0 ; P>Tの場合 右回転
SUBWF POTN,0 ; W=P-T
SUBLW d'1' ; W=1-(P-T) |T-P|=1 の不感帯を設ける
BTFSC STATUS,C ; C=0すなわち上記引き算がマイナスの時(P-T)>1、次スキップし右回転
GOTO MSTOP
MOVF TWID,0
SUBWF POTN,1 ; POTN=P-T
MOVLW d'2' ; 加速加算 3以上ではオーバーシュート気味になる
ADDWF POTN ; POTN=P-T+2
MOVLW d'14'
SUBWF POTN,0 ; W=P-T+2-14
BTFSS STATUS,C ; C=1すなわち上記引き算が>=0の時(P-T+2)>=14、次スキップ
GOTO RTN1
MOVLW d'14'
MOVWF POTN ; POTN=14 最大14ｍSec制限
RTN1 BCF GPIO,1
BCF GPIO,2
BSF GPIO,4
BSF GPIO,5
RTN CALL WAIT1mS ; (P-T+2)*1mSec (最大14ｍSec制限)の+5Vモーター駆動パルスを生成
DECFSZ POTN,1
GOTO RTN
GOTO MSTOP
;---------------------------
;受信機からの入力信号パルス幅測定routin （信号Puls形態： 1.0~1.5~2.0ｍｓ 休止幅15~20ｍｓ)
;--------------------------- ( 1~2mSec Spend )
SIGNALANAL CLRF TWID
CSIG BTFSS GPIO,3 ;GP3の+5V信号開始チェック +5V感知したら 次のステップはスキップ
GOTO CSIG
COUNT INCF TWID,1
NOP ; 10μsec間隔で+5V信号パルス変化幅を刻み測定
NOP ; 1+6+1+2=10
NOP
NOP
NOP
NOP
BTFSC GPIO,3 ;GP3の信号チェック 0V感知したら 次のステップはスキップ
GOTO COUNT
RETURN ;カウント終了してリターン TWID=100～150～200
;---------------------------
; ポテンショの電圧読み取りAD変換 routin (44μsec Spend) 2+2+26+1+9+2+2=44
;---------------------------
POTENTIO
MOVLW b'00000001'
MOVWF ADCON0 ;左詰出力、電源電圧基準、計測ピンはGP0、ADスタンバイ
CALL WAIT26 ; Need Charge Time 26μsec>20μsec
BSF ADCON0, GO
POTLP BTFSC ADCON0, GO ;AD完了チェック
GOTO POTLP
MOVF ADRESH, 0
MOVWF POTN ;AD結果の上位８ビットを POTN に格納 POTN=0～(100～150～200)～255
RETURN
;---------------------------
; 1mSec WAIT サブルーチン 2+2+(26+1+2)*35+2=1021
;---------------------------
WAIT1mS
MOVLW d'35'
MOVWF COUNT2
WLP1
CALL WAIT26
DECFSZ COUNT2, 1
GOTO WLP1
RETURN
;----------------------------
; 26μsec WAIT サブルーチン 2+2+(2+1+2)*4+2=26μSec
;----------------------------
WAIT26 ; 4MHzクロックなので 1実行サイクルは1μSec
MOVLW d'4'
MOVWF COUNT1
WLP NOP
NOP
DECFSZ COUNT1, 1
GOTO WLP
RETURN
END