;----------------  Elefant 2ch用 Ver.2 --------------------

;TAMIYA DMD-T02仕様
;砲身上下サーボのコントロール
;ブローバックサーボコントロール
;大砲音＋車体の揺れ
;機銃のＬＥＤの点滅速度を調整可能に
;Futaba の補正つき（－3）2002.8.14
;機銃可動　2002.10.16

;Ver2.0.0	2002.10.16	By T.HINOKI

		LIST		P=16C84,R=DEC
	PC	EQU	2
	STATUS	EQU	3
	PG	EQU	5
	PORTA	EQU	5
	PORTB	EQU	6
	EEDATA	EQU	8
	EEADR	EQU	9
	OPTION_REG	EQU	H'81'
	TRISA	EQU	H'85'
	TRISB	EQU	H'86'
	EECON1	EQU	H'88'
	EECON2	EQU	H'89'
	RD	EQU	0
	WR	EQU	1
	WREN	EQU	2
	COUNT1	EQU	H'10'
	COUNT2	EQU	H'11'
	PIN1	EQU	H'12'	;1ｃｈ入力パルス幅（左・右）
	PIN2	EQU	H'13'	;２ｃｈ入力パルス幅（前進・後進）
	COUNT12	EQU	H'14'	;砲塔旋回のタイミング

	POUT1	EQU	H'16'	;1ｃｈ出力パルス幅（左キャタ速度）
	POUT2	EQU	H'17'	;２ｃｈ出力パルス幅（右キャタ速度）
	POUT3	EQU	H'18'	;３ｃｈ出力パルス幅（砲身上下用）
	POUT4	EQU	H'19'	;４ｃｈ出力パルス幅（機銃可動用）
	POUT5	EQU	H'20'	;５ｃｈ出力パルス幅（ブローバックサーボ用）

	KEISAN1	EQU	H'1A'	;計算用変数
	KEISAN2	EQU	H'1B'	;計算用変数
	KEISAN3	EQU	H'1C'	;計算用変数
	HSPEED	EQU	H'1D'	;砲身の上下スピード
	COUNT3	EQU	H'1E'	;砲身の上下スピード作業用
	CENTER3	EQU	H'1F'	;機銃サーボ停止位置
	COUNT4	EQU	H'21'	;ブローバックサーボの動作カウント用
	COUNT10	EQU	H'22'	;長時間のウエイト用
	COUNT11	EQU	H'23'	;ＴＯＰスピードの時間（揺れに使用）
	COUNT9	EQU	H'24'	;デモ走行作業用
	COUNT8	EQU	H'25'	;機銃発光のタイミング
	CENTER1	EQU	H'26'	;走行用アンプのセンター位置(左)
	CENTER2	EQU	H'27'	;走行用アンプのセンター位置(右)
	SOKUDO	EQU	H'28'	;旋回時の速度ＵＰ値
	KIJUU	EQU	H'29'	;機銃可動方向　1　or　2

	ORG	0
START
	CLRF	PORTB		;クリア PORTB
	BSF	STATUS,PG	;Change Page1
	CLRF	TRISB		;TRISB=0  All output
	MOVLW	B'01111'	;RA4のみ出力
	MOVWF	TRISA		;RA0-3は入力
	BCF	STATUS,PG	;Change Page0

;--------------音・発光の初期値 ----------------
	BSF	PORTB,7		;音のスイッチ off
        MOVLW	B'10000'	;RA4のLEDをOFF
	MOVWF	PORTA		;
	BSF	PORTB,5		;LED off

	MOVLW	1		;発光のタイミング設定クリア
	MOVWF	COUNT8

	MOVLW	1
	CALL	TIM90

LOOP00A
	MOVLW	150		;アンプのセンターを１５０（停止）に設定
	MOVWF	CENTER1

	MOVLW	150		;アンプのセンターを１５０（停止）に設定
	MOVWF	CENTER2

	MOVLW	150		;機銃可動サーボを１５０（停止）に設定
	MOVWF	POUT4

	MOVLW	1		;機銃可動サーボを１５０（停止）に設定
	MOVWF	KIJUU		;機銃サーボの可動方向
LOOP000



;---------砲身のデフォルト位置＆上下スピード
LOOP01
	MOVLW	180		;スイッチを入れたときの砲身のデフォルト角度設定
	MOVWF	POUT3		;

	MOVLW	3		;砲身上下のスピード0-5
	MOVWF	HSPEED		;
	MOVWF	COUNT3		;

;-----------ブローバック用サーボ停止位置 ------------
	MOVLW	200		;
	MOVWF	POUT5
	MOVLW	255		;
	MOVWF	COUNT4

;-------------------メインルーチン -------------
LOOP1

	CALL	PULSIN1		;1chパルス幅取得
	CALL	PULSIN2		;2chパルス幅取得

	MOVF	PIN1,0
	MOVWF	POUT1		;
	MOVF	PIN2,0
	MOVWF	POUT2		;

;--------停止中

	MOVLW	152		;前後スティックが152以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=152はNEXT51へ
	GOTO	NEXT51
	MOVLW	148		;前後スティックが148以上だったら停止
	SUBWF	PIN2,0		;
	BTFSS	STATUS,0	;(CY=0) PIN2<148はNEXT51へ
	GOTO	NEXT51
	MOVLW	150		;左右中間(停止)に
	MOVWF	POUT1
	MOVWF	POUT2


;-----------------　音　---------------
	MOVLW	118		;1chスティックが118以下だったら
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=118はOTO10へ
	GOTO	OTO10		;

	BSF	STATUS,PG	;Change Page1
	BSF	TRISB,6		;RB6を入力に切替
	BCF	STATUS,PG	;Change Page0


	BCF	PORTB,7		;音のＳＷ on(機関銃音)

	DECFSZ	COUNT8,1	;COUNT8が0の時だけＬＥＤ点ける
	GOTO	OTO03
	BCF	PORTB,5		;LED on
	MOVLW	4		;発光のタイミング設定 1-10
	MOVWF	COUNT8

	CALL	PULSOFF		;発光の間隔の時間稼ぎ
	CALL	PULSOFF
	CALL	PULSOFF
;--------------------------   機銃可動   -----------------
OTO03
	MOVLW	111		;1chスティックが111より小さいときは機銃可動停止
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<111はOTO80へ
	GOTO	OTO80		;


	MOVF	KIJUU,0
	SUBLW	1
	BTFSC	STATUS,2
	GOTO	OTO05

	INCF	POUT4,1
	MOVLW	180		;180以上だったら（右）
	SUBWF	POUT4,0		;
	BTFSS	STATUS,0	;(CY=0) POUT4<180はOTO04へ
	GOTO	OTO04		;

	MOVLW	1		;可動方向変更
	MOVWF	KIJUU
OTO04
	GOTO	OTO80		;
OTO05
	DECF	POUT4,1
	MOVLW	130		;130以下だったら(左)
	SUBWF	POUT4,0		;
	BTFSC	STATUS,0	;(CY=1) POUT4>=120はOTO06へ
	GOTO	OTO06		;

	MOVLW	0		;可動方向変更
	MOVWF	KIJUU
OTO06
	GOTO	OTO80		;
;--------------------------------------------------------------
OTO10
	MOVLW	188		;1chスティックが188以上だったら
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<188はOTO20へ
	GOTO	OTO20		;

	MOVLW	255		;COUNT4<>255だったら
	SUBWF	COUNT4,0	;
	BTFSS	STATUS,2	;(Z=0) COUNT4<>255
	GOTO	OTO80

        MOVLW	B'00000'	;RA4のLEDをON
	MOVWF	PORTA		;

	BSF	STATUS,PG	;Change Page1
	BCF	TRISB,6		;RB6を出力に切替
	BCF	STATUS,PG	;Change Page0

	BSF	PORTB,6		;PULS6 on（音切り換え）
	CALL	TIM20		;ちょっと待つ
	BCF	PORTB,7		;音のＳＷ on（大砲音）

;-----------  砲身後退　-----------
	MOVLW	90		;砲身後退サーボの値を９０にして
	MOVWF	POUT5		;砲身を一挙に後退
	MOVLW	100		;後退した後の一瞬の停止時間の設定
	MOVWF	COUNT4		;10～100（100で停止無し）

;----------- 車体の揺れ？？ --------

	MOVLW	6		;バックの時間
	CALL	TOPSP
	MOVLW	7		;前進の時間
	CALL	BACKSP
	MOVLW	2		;バックの時間
	CALL	TOPSP

	CALL	PULSOFF


	BSF	PORTB,1		;PULS2 on
	MOVLW	150		;停止位置にに戻す
	CALL	TIM25
	BCF	PORTB,1		;PULS off

	GOTO	OTO80		;
OTO20
	BSF	PORTB,7		;音のＳＷ off
        MOVLW	B'10000'	;RA4のLEDをOFF
	MOVWF	PORTA		;制御実行

;-----------------　砲身上下　---------------
	MOVLW	140		;1chスティックが140以下だったら
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=140はHOUSIN10へ
	GOTO	HOUSIN10	;

	DECFSZ	COUNT3,1		;COUNT3=COUNT3-1
	GOTO	PULSOUT		;COUNT3>0はサーボはそのまま

	MOVF	HSPEED,0	;COUNT3＝HSPEED(砲身上下のスピード)
	MOVWF	COUNT3		;

	INCF	POUT3,1		;POUT3=POUT3+1

	MOVLW	195		;砲身上下サーボがすでに195以上だったら
	SUBWF	POUT3,0		;
	BTFSS	STATUS,0	;(CY=0) POUT3<195はPULSOUT10へ
	GOTO	OTO80

	MOVLW	195		;砲身上下サーボを195に固定
	MOVWF	POUT3		;POUT3=195
	GOTO	OTO80

HOUSIN10
	MOVLW	162		;1chスティックが162以上だったら
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<162はHOUSIN20へ
	GOTO	HOUSIN20	;

	DECFSZ	COUNT3,1		;COUNT3=COUNT3-1
	GOTO	PULSOUT		;COUNT3>0はサーボはそのまま

	MOVF	HSPEED,0	;COUNT3＝HSPEED(砲身上下のスピード)
	MOVWF	COUNT3		;

	DECF	POUT3,1		;POUT3=POUT3-1

	MOVLW	130		;砲身上下サーボがすでに130以下だったら
	SUBWF	POUT3,0		;
	BTFSC	STATUS,0	;(CY=1) POUT3>=130はHOUSIN20へ
	GOTO	OTO80		;

	MOVLW	130		;砲身上下サーボを130に固定
	MOVWF	POUT3		;POUT3=130
	GOTO	OTO80		;
HOUSIN20


OTO80
	GOTO	PULSOUT
;-------------------- 前進・後進中 --------------------
NEXT51

	BSF	PORTB,7		;音のＳＷ off

PULSOUT
;--------- 走行用パルス出力
	CALL	PULSOUTSUB

;--------砲身上下
	BSF	PORTB,2		;PULS3 on
	MOVF	POUT3,0
	CALL	TIM25
	BCF	PORTB,2		;PULS off
;--------- 機銃サーボ

	BSF	PORTB,3		;PULS4 on
	MOVF	POUT4,0
	CALL	TIM25
	BCF	PORTB,3		;PULS off
NEXT54

;------------　　砲身を戻す処理
	MOVLW	250		;250以上だったら
	SUBWF	COUNT4,0	;
	BTFSS	STATUS,0	;(CY=0) COUNT4<250は
	GOTO	PULSOUT31
	MOVLW	255		;
	MOVWF	COUNT4		;COUNT4=255
	GOTO	PULSOUT32
PULSOUT31
	MOVLW	4		;砲身を戻す時のスピード(1-5)
	ADDWF	COUNT4,1	;COUNT4=COUNT4+4
PULSOUT32
	MOVLW	200		;200以上だったら
	SUBWF	COUNT4,0	;
	BTFSS	STATUS,0	;(CY=0) COUNT4<200は
	GOTO	PULSOUT30	;
	MOVLW	200		;
	MOVWF	POUT5		;POUT5=200
	GOTO	PULSOUT40	;

PULSOUT30
	MOVLW	100		;100以上だったら
	SUBWF	COUNT4,0	;
	BTFSS	STATUS,0	;(CY=0) COUNT4<100は
	GOTO	PULSOUT40	;

	MOVF	COUNT4,0	;W=COUNT4
	MOVWF	POUT5		;POUT5=W

PULSOUT40
	BSF	PORTB,4		;PULS5 on
	MOVF	POUT5,0
	CALL	TIM25
	BCF	PORTB,4		;PULS off

	BSF	PORTB,5		;LED off

	GOTO	LOOP1

;==================走行用２アンプ出力のサブルーチン
PULSOUTSUB
	BSF	PORTB,0		;PULS1 on
	MOVF	POUT1,0
	CALL	TIM25
	BCF	PORTB,0		;PULS off

	CALL	PULSOFF

	BSF	PORTB,1		;PULS2 on
	MOVF	POUT2,0
	CALL	TIM25
	BCF	PORTB,1		;PULS off
	RETURN

;============================  機関銃音のサブルーチン
OTOSUB
	MOVWF	COUNT11		;
	BSF	STATUS,PG	;Change Page1
	BSF	TRISB,6		;RB6を入力に切替
	BCF	STATUS,PG	;Change Page0

	BCF	PORTB,7		;音のＳＷ on(機関銃音)

OTOSUB1
	BCF	PORTB,5		;LED on
	MOVLW	1	
	CALL	TIM90
	BSF	PORTB,5		;LED off
	MOVLW	1
	CALL	TIM90
	DECFSZ	COUNT11,1	;COUNT11が0になるまで繰り返す
	GOTO	OTOSUB1

	BSF	PORTB,7		;音のＳＷ off
	RETURN			;

;============================  大砲発射のサブルーチン
TAIHO

        MOVLW	B'00000'	;RA4のLEDをON
	MOVWF	PORTA		;制御実行
	MOVLW	3
	CALL	TIM90

	BSF	STATUS,PG	;Change Page1
	BCF	TRISB,6		;RB6を出力に切替
	BCF	STATUS,PG	;Change Page0

	BSF	PORTB,6		;on（音切り換え）
	CALL	TIM20		;ちょっと待つ
	BCF	PORTB,7		;音のＳＷ on（大砲音）


;-----------  砲身後退
	MOVLW	90		;砲身後退サーボの値を９０にして
	MOVWF	POUT5		;砲身を一挙に後退

	MOVLW	70		;後退した後の一瞬の停止時間の設定
	MOVWF	COUNT4		;80～100（100で停止無し）

	BSF	PORTB,4		;PULS5 on
	MOVF	POUT5,0
	CALL	TIM25
	BCF	PORTB,4		;PULS off

	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF

;----------- 車体の揺れ

	MOVLW	15		;前進の時間
	CALL	TOPSP
	MOVLW	15		;バックの時間
	CALL	BACKSP
	MOVLW	15		;前進の時間
	CALL	TOPSP
	MOVLW	10		;バックの時間
	CALL	BACKSP
	MOVLW	10		;前進の時間
	CALL	TOPSP

	CALL	PULSOFF

	BSF	PORTB,1		;PULS2 on
	MOVLW	150		;停止位置にに戻す
	CALL	TIM25
	BCF	PORTB,1		;PULS off

        MOVLW	B'10000'	;RA4のLEDをOFF制御
	MOVWF	PORTA		;


;------------　　砲身を戻す処理
TAIHO01
	MOVLW	201		;200以上だったら
	SUBWF	COUNT4,0	;
	BTFSS	STATUS,0	;(CY=0) COUNT4<201は
	GOTO	TAIHO02
	MOVLW	255		;
	MOVWF	COUNT4		;COUNT4=255

	MOVLW	20
	CALL	TIM90

	BSF	PORTB,7		;PULS off（大砲音）

	RETURN

TAIHO02
	MOVLW	1		;
	ADDWF	COUNT4,1	;COUNT4=COUNT4+1

TAIHO03
	MOVLW	100		;100以上だったら
	SUBWF	COUNT4,0	;
	BTFSS	STATUS,0	;(CY=0) COUNT4<100は
	GOTO	TAIHO04	;

	MOVF	COUNT4,0	;W=COUNT4
	MOVWF	POUT5		;POUT5=W

TAIHO04
	CALL	PULSOFF
	CALL	PULSOFF
	BSF	PORTB,4		;PULS5 on
	MOVF	POUT5,0
	CALL	TIM25
	BCF	PORTB,4		;PULS off

	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	GOTO	TAIHO01


;====================== 砲身上下 =====================
JYOGE
	MOVWF	COUNT11
	MOVLW	10
	MOVWF	COUNT10

JYOGE01
	MOVF	COUNT11,0
	SUBWF	POUT3,0
	BTFSS	STATUS,0	;(CY=0) POUT3<Wは
	GOTO	JYOGE02

	MOVF	COUNT11,0
	SUBWF	POUT3,0
	BTFSC	STATUS,2	;(Z=1) POUT3=Wは
	RETURN

	DECFSZ	COUNT10,1	;COUNT10が0になるまで繰り返す
	GOTO	JYOGE04

	MOVLW	1
	SUBWF	POUT3,1		;POUT3=POUT3-1
	MOVLW	10
	MOVWF	COUNT10
	GOTO	JYOGE04

JYOGE02
	DECFSZ	COUNT10,1	;COUNT10が0になるまで繰り返す
	GOTO	JYOGE04

	MOVLW	1		;
	ADDWF	POUT3,1		;POUT3=POUT3+1
	MOVLW	10
	MOVWF	COUNT10


JYOGE04
	BSF	PORTB,2		;PULS3 on
	MOVF	POUT3,0
	CALL	TIM25
	BCF	PORTB,2		;PULS off
	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	GOTO	JYOGE01


;-------------  20サイクルタイマー（実際は19サイクル）
TIM20
	GOTO	$+1	;2
	GOTO	$+1	;+2=4
	GOTO	$+1	;+2=6
	GOTO	$+1	;+2=8
	GOTO	$+1	;+2=10
	GOTO	$+1	;+2=12
	GOTO	$+1	;+2=14
	GOTO	$+1	;+2=16
	NOP		;+1=17
	RETURN		;+2=19

;-------------  Ｗレジスタ分のカウントタイマー(1カウント=25サイクル)×Ｗ
TIM25
	MOVWF	COUNT1		;1+1=2
TIM26
	CALL	TIM20		;19+2=21
	NOP			;+1=22
	DECFSZ	COUNT1,1	;+1=23
	GOTO	TIM26		;+2=25
	RETURN			;+1+(W*25)+2-1

;-----------パルスとパルスの間の空き時間-----------------
PULSOFF
	MOVLW	150
	CALL	TIM25		;
	RETURN

;-------------  時間稼ぎ(1/4秒×Ｗ)
TIM90
	MOVWF	COUNT10		;
TIM91
	MOVLW	50
	MOVWF	COUNT9		;
TIM92
	MOVLW	255
	CALL	TIM25		;
	DECFSZ	COUNT9,1	;
	GOTO	TIM92		;
	DECFSZ	COUNT10,1	;
	GOTO	TIM91		;
	RETURN			;

;-------------  揺れ（一瞬ＴＯＰに）
TOPSP
	MOVWF	COUNT11		;

TOPSP1

	BSF	PORTB,1		;PULS2 on
	MOVLW	173		;
	CALL	TIM25
	BCF	PORTB,1		;PULS off

	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF

	BSF	PORTB,0		;PULS1 on
	MOVLW	150		;
	CALL	TIM25
	BCF	PORTB,0		;PULS off

	BSF	PORTB,4		;PULS5 on
	MOVF	POUT5,0
	CALL	TIM25		;砲身後退
	BCF	PORTB,4		;PULS off

	MOVLW	100
	CALL	TIM25		;

	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	DECFSZ	COUNT11,1	;COUNT11が0になるまで繰り返す
	GOTO	TOPSP1		;
	RETURN			;

;-------------  揺れ（一瞬バックに）
BACKSP
	MOVWF	COUNT11		;

BACKSP1

	BSF	PORTB,1		;PULS2 on
	MOVLW	135		;
	CALL	TIM25
	BCF	PORTB,1		;PULS off

	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF

	BSF	PORTB,0		;PULS1 on
	MOVLW	150		;
	CALL	TIM25
	BCF	PORTB,0		;PULS off

	BSF	PORTB,4		;PULS5 on
	MOVF	POUT5,0
	CALL	TIM25		;砲身後退
	BCF	PORTB,4		;PULS off

	MOVLW	100
	CALL	TIM25		;
	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	CALL	PULSOFF
	DECFSZ	COUNT11,1	;COUNT11が0になるまで繰り返す
	GOTO	BACKSP1		;
	RETURN			;

;-------- ＥＥＰＲＯＭ書き込み
EEPW
	MOVWF	EEADR		;
	BSF	STATUS,PG	;Change Page1
	BSF	EECON1,WREN	;EECON1のWREN=1(書込許可)
	MOVLW	H'55'
	MOVWF	EECON2		;書込開始
	MOVLW	H'AA'
	MOVWF	EECON2
	BSF	EECON1,WR	;
WR_LP
	BTFSC	EECON1,WR	;書込終了チェック
	GOTO	WR_LP
	BCF	STATUS,PG	;Change Page0
	RETURN
;-----------1CHパルス幅測定（PIN1）前進・後進のスティックの信号チェック-------
PULSIN1A
	BTFSS	PORTA,0		;OFF の時
	GOTO	PULSIN1		;入力開始
	CALL	TIM20
	GOTO	PULSIN1A	;次の信号まで待つ
;-----------1CHパルス幅測定（PIN1）左右のステック-----------
PULSIN1
	BTFSS	PORTA,0		;OFF の時
	GOTO	PULSIN1		;入力待ちループ
	MOVLW	0		;
	MOVWF	PIN1		;カウンター初期化
PULSIN11
	CALL	TIM20		;20サイクル待ち	(19+2)
	INCF	PIN1,1		;PIN1=PIN1+1	(22)
	BTFSC	PORTA,0		;ＯＮの時は	(23)
	GOTO	PULSIN11	;カウント継続	(25)

	MOVLW	90		;カウントが90以下だったら
	SUBWF	PIN1,0		;カウントしなおし！
	BTFSS	STATUS,0	;(CY=0) PIN1<90
	GOTO	PULSIN1		;
;Futabaの補正　－3
	MOVLW	3
	SUBWF	PIN1,1		;PIN1=PIN1-3

	RETURN
;-----------2CHパルス幅測定（PIN2）前進・後進のスティックの信号チェック-------
PULSIN2A
	BTFSS	PORTA,1		;OFF の時
	GOTO	PULSIN2		;入力開始
	CALL	TIM20
	GOTO	PULSIN2A	;次の信号まで待つ

;-----------2CHパルス幅測定（PIN2）前進・後進のスティック-----------
PULSIN2
	BTFSS	PORTA,1		;OFF の時
	GOTO	PULSIN2		;入力待ちループ
	MOVLW	0		;
	MOVWF	PIN2		;カウンター初期化
PULSIN21
	CALL	TIM20		;20サイクル待ち
	INCF	PIN2,1		;PIN1=PIN1+1
	BTFSC	PORTA,1		;ＯＮの時は、
	GOTO	PULSIN21	;カウント継続

	MOVLW	90		;カウントが90以下だったら
	SUBWF	PIN2,0		;カウントしなおし！
	BTFSS	STATUS,0	;(CY=0) PIN2<90
	GOTO	PULSIN2		;
;Futabaの補正　－3
	MOVLW	3
	SUBWF	PIN2,1		;PIN2=PIN2-3

	RETURN


	END
