;----------------  Famo用 Ver.1 --------------------

;２サーボ（アンプ）コントロールプログラム
;サーボの強制停止機能つき
;左右旋回値をデータ化(Ver.8)
;走行用アンプのセンター位置を記憶(Ver.9)
;左右旋回値をデータを１０×１０に
;左右旋回時の片側補正値を追加
;超新地旋回なし
;Ver1.0.0	2001.05.8	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'	;２ｃｈ入力パルス幅（前進・後進）
	PIN3	EQU	H'14'	;３ｃｈ入力パルス幅（砲身上下用）
	PIN4	EQU	H'15'	;４ｃｈ入力パルス幅（砲塔旋回用）

	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'	;旋回時の速度ＵＰ値

	ORG	0
	GOTO	START
;------------------------減速ＤＡＴＡ---------------------
	ORG	3
SSOKUDO
	ADDWF	PC,1	;X.Y
	RETLW	4	;0.0
	RETLW	4	;0.1
	RETLW	3	;0.2
	RETLW	3	;0.3
	RETLW	3	;0.4
	RETLW	2	;0.5
	RETLW	2	;0.6
	RETLW	1	;0.7
	RETLW	1	;0.8
	RETLW	1	;0.9
	RETLW	6	;1.0
	RETLW	5	;1.1
	RETLW	4	;1.2
	RETLW	4	;1.3
	RETLW	3	;1.4
	RETLW	3	;1.5
	RETLW	2	;1.6
	RETLW	2	;1.7
	RETLW	1	;1.8
	RETLW	1	;1.9
	RETLW	10	;2.0
	RETLW	9	;2.1
	RETLW	8	;2.2
	RETLW	7	;2.3
	RETLW	6	;2.4
	RETLW	5	;2.5
	RETLW	4	;2.6
	RETLW	3	;2.7
	RETLW	2	;2.8
	RETLW	1	;2.9
	RETLW	15	;3.0
	RETLW	15	;3.1
	RETLW	14	;3.2
	RETLW	12	;3.3
	RETLW	10	;3.4
	RETLW	8	;3.5
	RETLW	6	;3.6
	RETLW	4	;3.7
	RETLW	2	;3.8
	RETLW	1	;3.9
	RETLW	20	;4.0
	RETLW	20	;4.1
	RETLW	17	;4.2
	RETLW	14	;4.3
	RETLW	12	;4.4
	RETLW	10	;4.5
	RETLW	7	;4.6
	RETLW	4	;4.7
	RETLW	3	;4.8
	RETLW	2	;4.9
	RETLW	25	;5.0
	RETLW	24	;5.1
	RETLW	19	;5.2
	RETLW	16	;5.3
	RETLW	13	;5.4
	RETLW	11	;5.5
	RETLW	7	;5.6
	RETLW	4	;5.7
	RETLW	3	;5.8
	RETLW	2	;5.9
	RETLW	30	;6.0
	RETLW	29	;6.1
	RETLW	24	;6.2
	RETLW	20	;6.3
	RETLW	16	;6.4
	RETLW	12	;6.5
	RETLW	8	;6.6
	RETLW	5	;6.7
	RETLW	4	;6.8
	RETLW	3	;6.9
	RETLW	35	;7.0
	RETLW	34	;7.1
	RETLW	29	;7.2
	RETLW	24	;7.3
	RETLW	19	;7.4
	RETLW	14	;7.5
	RETLW	9	;7.6
	RETLW	6	;7.7
	RETLW	4	;7.8
	RETLW	3	;7.9
	RETLW	40	;8.0
	RETLW	38	;8.1
	RETLW	32	;8.2
	RETLW	26	;8.3
	RETLW	20	;8.4
	RETLW	15	;8.5
	RETLW	10	;8.6
	RETLW	7	;8.7
	RETLW	6	;8.8
	RETLW	4	;8.9
	RETLW	45	;9.0
	RETLW	40	;9.1
	RETLW	35	;9.2
	RETLW	30	;9.3
	RETLW	25	;9.4
	RETLW	20	;9.5
	RETLW	15	;9.6
	RETLW	10	;9.7
	RETLW	7	;9.8
	RETLW	4	;9.9
	RETLW	45	;9.0
	RETLW	40	;9.1
	RETLW	35	;9.2
	RETLW	30	;9.3
	RETLW	25	;9.4
	RETLW	20	;9.5
	RETLW	15	;9.6
	RETLW	10	;9.7
	RETLW	7	;9.8
	RETLW	4	;9.9
;------------------------　片側減速時の反対側補正値　-------
SSOKUDO2
	ADDWF	PC,1	;X
	RETLW	1	;0
	RETLW	2	;1
	RETLW	3	;2
	RETLW	4	;3
	RETLW	5	;4
	RETLW	6	;5
	RETLW	7	;6
	RETLW	8	;7
	RETLW	9	;8
	RETLW	0	;9
	RETLW	0	;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


	MOVLW	1
	CALL	TIM90

;------------------ ＥＥＰＲＯＭ読込
	MOVLW	0		;EPPROMの0番地（先頭）のＤＡＴＡを読む
	MOVWF	EEADR		;
	BSF	STATUS,PG	;Change Page1
	BSF	EECON1,RD	;読出し開始
	BCF	STATUS,PG	;Change Page0
	MOVF	EEDATA,0
	MOVWF	CENTER3		;CENTER3=W	砲塔旋回サーボセンターの位置を記憶

	MOVLW	1		;EPPROMの1番地のＤＡＴＡを読む
	MOVWF	EEADR		;
	BSF	STATUS,PG	;Change Page1
	BSF	EECON1,RD	;読出し開始
	BCF	STATUS,PG	;Change Page0
	MOVF	EEDATA,0
	MOVWF	CENTER1		;CENTER1=W	アンプセンターの位置を記憶

	MOVLW	2		;EPPROMの2番地のＤＡＴＡを読む
	MOVWF	EEADR		;
	BSF	STATUS,PG	;Change Page1
	BSF	EECON1,RD	;読出し開始
	BCF	STATUS,PG	;Change Page0
	MOVF	EEDATA,0
	MOVWF	CENTER2		;CENTER2=W	アンプセンターの位置を記憶

;-------ＲＯＭ値のチェック（走行用アンプ）
	MOVLW	170		;
	SUBWF	CENTER1,0	;
	BTFSC	STATUS,0	;(CY=1) CENTER1>=170はLOOP00へ
	GOTO	LOOP00A		;
	MOVLW	130		;
	SUBWF	CENTER1,0	;
	BTFSS	STATUS,0	;(CY=0) CENTER1<120はLOOP00へ
	GOTO	LOOP00A		;
	GOTO	LOOP000A
LOOP00A
	MOVLW	150		;アンプのセンターを１５０（停止）に設定
	MOVWF	CENTER1

LOOP000A
	MOVLW	170		;
	SUBWF	CENTER2,0	;
	BTFSC	STATUS,0	;(CY=1) CENTER2>=170はLOOP00へ
	GOTO	LOOP00B		;
	MOVLW	130		;
	SUBWF	CENTER2,0	;
	BTFSS	STATUS,0	;(CY=0) CENTER2<120はLOOP00へ
	GOTO	LOOP00B		;
	GOTO	LOOP000B
LOOP00B
	MOVLW	150		;アンプのセンターを１５０（停止）に設定
	MOVWF	CENTER2

LOOP000B

;-------ＲＯＭ値のチェック(砲塔旋回サーボの中心)
	MOVLW	180		;
	SUBWF	CENTER3,0	;
	BTFSC	STATUS,0	;(CY=1) CENTER3>=180はLOOP00へ
	GOTO	LOOP00		;
	MOVLW	120		;
	SUBWF	CENTER3,0	;
	BTFSS	STATUS,0	;(CY=0) CENTER3<120はLOOP00へ
	GOTO	LOOP00		;
	GOTO	LOOP000
LOOP00
	MOVLW	150		;砲塔旋回値を１５０（停止）に設定
	MOVWF	CENTER3

LOOP000


;********走行用アンプ＆砲塔旋回サーボの停止位置設定（電源投入時、前進後進スティックが１番下の時に設定登録）

	MOVLW	1
	CALL	TIM90
	CALL	PULSOFF		;電源入れていきなり受け取るとノイズがあるかも...ちょっと待とう
	CALL	PULSIN2A	;2chパルス幅取得

	MOVLW	180		;前後スティックが180以上だったら設定開始
	SUBWF	PIN2,0		;
	BTFSS	STATUS,0	;(CY=0) PIN2<180はLOOP01へ
	GOTO	LOOP01		;

	BCF	PORTB,5		;LED on（設定中は機銃用ＬＥＤを点灯）
        MOVLW	B'00000'	;RA4のLEDもON
	MOVWF	PORTA		;

LOOP0
	CALL	PULSIN4A		;4chパルス幅取得
	CALL	PULSIN2A		;2chパルス幅取得
	CALL	PULSIN3A		;3chパルス幅取得
	MOVF	PIN2,0		;W=PIN2
	MOVWF	CENTER1		;CENTER1=W	センターの位置を記憶
	MOVF	PIN3,0		;W=PIN3
	MOVWF	CENTER2		;CENTER2=W	センターの位置を記憶

	CALL	PULSIN1A		;1chパルス幅取得

	MOVF	PIN4,0		;-------  プロポの左右スティック値を
	MOVWF	POUT4		;-------  そのまま砲塔旋回サーボへ出力
	BSF	PORTB,3		;PULS3 on
	MOVF	POUT4,0		;
	CALL	TIM25		;
	BCF	PORTB,3		;PULS off
;
	BSF	PORTB,0		;PULS1 on
	MOVF	CENTER1,0	;W=CENTER1
;	MOVLW	150		;アンプのセンター位置
	CALL	TIM25		;
	BCF	PORTB,0		;PULS off
;
	BSF	PORTB,1		;PULS2 on
	MOVF	CENTER2,0	;W=CENTER2
;	MOVF	150		;アンプのセンター位置
	CALL	TIM25		;
	BCF	PORTB,1		;PULS off

	CALL	PULSOFF

	MOVLW	120		;左右スティックが120以下だったら設定終了
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=120はLOOP0へ
	GOTO	LOOP0		;

	BSF	PORTB,5		;LED off（機銃用のＬＥＤを消す）
        MOVLW	B'10000'	;RA4のLEDをOFF
	MOVWF	PORTA		;
	MOVF	PIN4,0		;W=PIN4
	MOVWF	CENTER3		;CENTER3=W	センターの位置を記憶

;-------- ＥＥＰＲＯＭ書き込み
	MOVWF	EEDATA		;EEDATA=W
	MOVLW	0		;
	CALL	EEPW

	MOVF	CENTER1,0	;W=CENTER1
	MOVWF	EEDATA		;EEDATA=W
	MOVLW	1		;
	CALL	EEPW

	MOVF	CENTER2,0	;W=CENTER2
	MOVWF	EEDATA		;EEDATA=W
	MOVLW	2		;
	CALL	EEPW


LOOP01

;-------------------メインルーチン -------------
LOOP1

	CALL	PULSIN1		;1chパルス幅取得
	CALL	PULSIN2		;2chパルス幅取得
	CALL	PULSOUTSUB	;受信機からパルスを受け取る時間が長すぎるので、途中でアンプ出力する
	CALL	PULSIN3		;3chパルス幅取得
	CALL	PULSIN4		;4chパルス幅取得

	MOVF	PIN2,0
	MOVWF	POUT1
	MOVWF	POUT2


	MOVF	CENTER3,0	;W=CENTER3
	MOVWF	POUT4		;POUT4=W	砲塔旋回サーボを停止


;--------停止中

	MOVLW	152		;前後スティックが153以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=152はNEXT51へ
	GOTO	NEXT51
	MOVLW	148		;前後スティックが147以上だったら停止
	SUBWF	PIN2,0		;
	BTFSS	STATUS,0	;(CY=0) PIN2<147はNEXT51へ
	GOTO	NEXT51
	MOVF	CENTER1,0	;W=CENTER1
	MOVWF	POUT1
	MOVF	CENTER2,0	;W=CENTER2
	MOVWF	POUT2
	GOTO	PULSOUT
NEXT51
;--------左旋回
	MOVLW	148		;左右スティックが147以下だったら
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=148はNEXT7へ
	GOTO	NEXT7		;
;--------前進
	MOVLW	150		;前後スティックが150以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=150はNEXT62へ
	GOTO	NEXT62		;

	MOVF	POUT1,0		;W=POUT1
	SUBLW	150		;W=150-速度
	CALL	WARU10		;((150-速度)÷5)×10
	MOVWF	KEISAN3		;KEISAN3=W

	MOVF	PIN1,0		;W=PIN1
	SUBLW	150		;150-角度
	CALL	WARU5		;(150-角度)÷5
	MOVF	KEISAN2,0	;W=KEISAN2
	ADDWF	KEISAN3,1	;KEISAN3=KEISAN3+W
	MOVF	KEISAN3,0	;W=KEISAN3
	CALL	SSOKUDO		;データ読み込み

	MOVWF	KEISAN3		;KEISAN3=W
	MOVF	CENTER1,0	;W=CENTER1
;	MOVLW	150		;W=150
	MOVWF	POUT1		;POUT1=W
	MOVF	KEISAN3,0	;W=KEISAN3
	SUBWF	POUT1,1		;POUT1=POUT1-W
	MOVF	SOKUDO,0	;W=SOKUDO	速度補正
	SUBWF	POUT2,1		;POUT2=POUT2-W


	GOTO	PULSOUT

NEXT62
;--------後進
	MOVLW	150		;W=150
	SUBWF	POUT1,0		;W=速度-150

	CALL	WARU10		;((速度-150)÷5)×10
	MOVWF	KEISAN3		;KEISAN3=W

	MOVF	PIN1,0
	SUBLW	150		;150-角度
	CALL	WARU5		;(150-角度)÷5
	MOVF	KEISAN2,0	;W=KEISAN2
	ADDWF	KEISAN3,1	;KEISAN3=KEISAN3+W
	MOVF	KEISAN3,0	;W=KEISAN3
	CALL	SSOKUDO		;データ読み込み

	MOVWF	POUT1		;POUT1=W
	MOVF	CENTER1,0	;W=CENTER1
;	MOVLW	150		;W=150
	ADDWF	POUT1,1		;POUT1=POUT1+W

	MOVF	SOKUDO,0	;W=SOKUDO	速度補正
	ADDWF	POUT2,1		;POUT2=POUT2+W

	GOTO	PULSOUT

NEXT7

;--------右旋回
	MOVLW	153		;左右スティックが153以上だったら
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<153はNEXT8へ
	GOTO	NEXT8		;

;--------前進
	MOVLW	150		;前後スティックが150以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=150はNEXT72へ
	GOTO	NEXT72		;

	MOVF	POUT2,0
	SUBLW	150		;150-速度

	CALL	WARU10		;((150-速度)÷5)×10
	MOVWF	KEISAN3		;KEISAN3=W

	MOVLW	150
	SUBWF	PIN1,0		;角度-150
	CALL	WARU5		;(角度-150)÷5
	MOVF	KEISAN2,0	;W=KEISAN2
	ADDWF	KEISAN3,1	;KEISAN3=KEISAN3+W
	MOVF	KEISAN3,0	;W=KEISAN3
	CALL	SSOKUDO		;データ読み込み

	MOVWF	KEISAN3		;KEISAN3=W
	MOVF	CENTER2,0	;W=CENTER2
;	MOVLW	150		;W=150
	MOVWF	POUT2		;POUT2=W
	MOVF	KEISAN3,0	;W=KEISAN3
	SUBWF	POUT2,1		;POUT2=POUT2-W

	MOVF	SOKUDO,0	;W=SOKUDO	速度補正
	SUBWF	POUT1,1		;POUT1=POUT1-W
	GOTO	PULSOUT

NEXT72
;--------後進
	MOVLW	150		;W=150
	SUBWF	POUT1,0		;速度-150

	CALL	WARU10		;((速度-150)÷5)×10
	MOVWF	KEISAN3		;KEISAN3=W

	MOVLW	150
	SUBWF	PIN1,0		;角度-150
	CALL	WARU5		;(角度-150)÷5

	MOVF	KEISAN2,0	;W=KEISAN2
	ADDWF	KEISAN3,1	;KEISAN3=KEISAN3+W
	MOVF	KEISAN3,0	;W=KEISAN3
	CALL	SSOKUDO		;データ読み込み

	MOVWF	POUT2		;POUT2=W
	MOVF	CENTER2,0	;W=CENTER2
;	MOVLW	150		;W=150
	ADDWF	POUT2,1		;POUT2=POUT2+W

	MOVF	SOKUDO,0	;W=SOKUDO	速度補正
	ADDWF	POUT1,1		;POUT1=POUT1+W
	GOTO	PULSOUT

NEXT8

PULSOUT
;--------- 走行用パルス出力
	CALL	PULSOUTSUB
	GOTO	LOOP1

;==================走行用２アンプ出力のサブルーチン
PULSOUTSUB
;--------停止中はパルスを出さない
	MOVLW	154		;前後スティックが154以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=154はNEXT51へ
	GOTO	PULSOUTSUB1	;
	MOVLW	146		;前後スティックが146以上だったら停止
	SUBWF	PIN2,0		;
	BTFSS	STATUS,0	;(CY=0) PIN2<146はNEXT51へ
	GOTO	PULSOUTSUB1	;
	RETURN
PULSOUTSUB1
	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



;***********　Ｗ÷５＝Ｗ
WARU5
	MOVWF	KEISAN1		;Ｗ=KEISAN1
	MOVLW	0		;
	MOVWF	KEISAN2		;カウンター初期化
	MOVF	KEISAN1,0	;W=KEISAN1
	SUBLW	4
	BTFSS	STATUS,0	;W>4
	GOTO	WARU51
	RETURN
WARU51
	MOVLW	5		;
	SUBWF	KEISAN1,1
	BTFSS	STATUS,0	;CY=0　で終了
	RETURN
	INCF	KEISAN2,1	;KEISAN2=KEISAN2+1
	GOTO	WARU51

;***********　(Ｗ÷5)×10＝KEISAN2
WARU10
	MOVWF	KEISAN1		;KEISAN1=W
	MOVLW	0		;
	MOVWF	KEISAN2		;カウンター初期化
	MOVF	KEISAN1,0	;W=KEISAN1
	SUBLW	4
	BTFSS	STATUS,0	;W>4
	GOTO	WARU101
	MOVLW	0		;
	MOVWF	SOKUDO		;速度＋分を0
	RETURN

WARU101
	MOVLW	5		;
	SUBWF	KEISAN1,1
	BTFSS	STATUS,0	;CY=0　で終了
	GOTO	KAKEL5
	INCF	KEISAN2,1	;KEISAN2=KEISAN2+1
	GOTO	WARU101

;***********　KEISAN2×10＝W
KAKEL5
	MOVF	KEISAN2,0	;W=KEISAN2
	CALL	SSOKUDO2
	MOVWF	SOKUDO		;SOKUDO=W

	MOVLW	0		;
	MOVWF	KEISAN1		;カウンター初期化
KAKELU51
	DECFSZ	KEISAN2,1	;KEISAN2=KEISAN2-1
	GOTO	KAKELU52	;KEISAN2が0になるまで繰り返し
	MOVF	KEISAN1,0	;W=KEISAN1
	RETURN
KAKELU52
	MOVLW	10		;
	ADDWF	KEISAN1,1		;KEISAN1=KEISAN1+W
	GOTO	KAKELU51


;-------------  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		;
;	MOVLW	255
;	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,0		;PULS1 on
	MOVLW	200		;
	CALL	TIM25
	BCF	PORTB,0		;PULS off

	BSF	PORTB,1		;PULS2 on
	MOVLW	200		;
	CALL	TIM25
	BCF	PORTB,1		;PULS off

	CALL	PULSOFF
	CALL	PULSOFF
	DECFSZ	COUNT11,1	;COUNT11が0になるまで繰り返す
	GOTO	TOPSP1		;
	RETURN			;

;-------------  揺れ（一瞬バックに）
BACKSP
	MOVWF	COUNT11		;
BACKSP1
	BSF	PORTB,0		;PULS1 on
	MOVLW	100		;
	CALL	TIM25
	BCF	PORTB,0		;PULS off

	BSF	PORTB,1		;PULS2 on
	MOVLW	100		;
	CALL	TIM25
	BCF	PORTB,1		;PULS off

	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		;
	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		;
	RETURN
;-----------3CHパルス幅測定（PIN3）前進・後進のスティックの信号チェック-------
PULSIN3A
	BTFSS	PORTA,2		;OFF の時
	GOTO	PULSIN3		;入力開始
	CALL	TIM20
	GOTO	PULSIN3A	;次の信号まで待つ

;-----------3CHパルス幅測定（PIN3）砲身上下用-----------
PULSIN3
	BTFSS	PORTA,2		;OFF の時
	GOTO	PULSIN3		;入力待ちループ
	MOVLW	0		;
	MOVWF	PIN3		;カウンター初期化
PULSIN31
	CALL	TIM20		;20サイクル待ち
	INCF	PIN3,1		;PIN3=PIN3+1
	BTFSC	PORTA,2		;ＯＮの時は、
	GOTO	PULSIN31	;カウント継続

	MOVLW	70		;カウントが70以下だったら
	SUBWF	PIN3,0		;カウントしなおし！
	BTFSS	STATUS,0	;(CY=0) PIN3<70
	GOTO	PULSIN3		;
	RETURN
;-----------4CHパルス幅測定（PIN4）前進・後進のスティックの信号チェック-------
PULSIN4A
	BTFSS	PORTA,3		;OFF の時
	GOTO	PULSIN4		;入力開始
	CALL	TIM20
	GOTO	PULSIN4A	;次の信号まで待つ

;-----------4CHパルス幅測定（PIN4）砲塔旋回用-----------
PULSIN4
	BTFSS	PORTA,3		;OFF の時
	GOTO	PULSIN4		;入力待ちループ
	MOVLW	0		;
	MOVWF	PIN4		;カウンター初期化
PULSIN41
	CALL	TIM20		;20サイクル待ち
	INCF	PIN4,1		;PIN4=PIN4+1
	BTFSC	PORTA,3		;ＯＮの時は、
	GOTO	PULSIN41	;カウント継続

	MOVLW	70		;カウントが70以下だったら
	SUBWF	PIN4,0		;カウントしなおし！
	BTFSS	STATUS,0	;(CY=0) PIN4<70
	GOTO	PULSIN4		;
	RETURN


	END