; RCタンクコントローラー「ひのき1型」4ch仕様　　2005.5.25
; MP4212アンプ×３　PIC 16F873A 20MHz 


	LIST	P=16F873A,F=INHX8M,R=DEC	;PICの種別指定
	INCLUDE	"P16F873A.INC"	;システム定数ファイルの連結
	;***コンフィギュレーション設定
	__CONFIG _HS_OSC & _WDT_OFF & _PWRTE_ON & _CP_OFF & _LVP_OFF & _DEBUG_OFF & _BODEN_OFF

	COUNT1	EQU	H'20'
	COUNT2	EQU	H'21'
	PIN1	EQU	H'22'	;1ｃｈ入力パルス幅（左・右）
	PIN2	EQU	H'23'	;２ｃｈ入力パルス幅（前進・後進）
	PIN3	EQU	H'24'	;３ｃｈ入力パルス幅（砲身上下用）
	PIN4	EQU	H'25'	;４ｃｈ入力パルス幅（砲塔旋回用）

	POUT1	EQU	H'26'	;1ｃｈ出力パルス幅（左キャタ速度）
	POUT2	EQU	H'27'	;２ｃｈ出力パルス幅（右キャタ速度）
	POUT3	EQU	H'28'	;３ｃｈ出力パルス幅（砲塔旋回用）
	POUT4	EQU	H'29'	;４ｃｈ出力パルス幅（砲身上下用）
	POUT5	EQU	H'2A'	;５ｃｈ出力パルス幅（ブローバックサーボ用）

	KEISAN1	EQU	H'2B'	;計算用変数
	KEISAN2	EQU	H'2C'	;計算用変数
	KEISAN3	EQU	H'2D'	;計算用変数
	HSPEED	EQU	H'2E'	;砲身の上下スピード
	COUNT3	EQU	H'2F'	;砲身の上下スピード作業用
	CENTER3	EQU	H'30'	;砲塔旋回サーボ停止位置
	COUNT4	EQU	H'31'	;ブローバックサーボの動作カウント用
	COUNT10	EQU	H'32'	;長時間のウエイト用
	COUNT11	EQU	H'33'	;ＴＯＰスピードの時間（揺れに使用）
	COUNT9	EQU	H'34'	;デモ走行作業用
	COUNT8	EQU	H'35'	;一定期間大砲が撃てないタイマー
	CENTER1	EQU	H'36'	;走行用アンプのセンター位置(左)
	CENTER2	EQU	H'37'	;走行用アンプのセンター位置(右)
	SOKUDO	EQU	H'38'	;旋回時の速度ＵＰ値
	PIN1B	EQU	H'39'	;1ｃｈモーター可動時間（0-50）
	PIN2B	EQU	H'3A'	;２ｃｈモーター可動時間（0-50）
	PIN3B	EQU	H'3B'	;３ｃｈモーター可動時間（0-50）
	PIN4B	EQU	H'3C'	;４ｃｈモーター可動時間（0-50）

	TIME1	EQU	H'3D'	;モーター可動タイマー1
	TIME2	EQU	H'3E'	;モーター可動タイマー2
	COUNT12	EQU	H'3F'	;振動時のカウンタ
	COUNT13	EQU	H'40'	;砲身の上下の作業用カウンタ
	COUNT14	EQU	H'41'	;砲身旋回の作業用カウンタ

	PIN1C	EQU	H'42'	;1ｃｈ入力パルス幅（左・右）
	PIN2C	EQU	H'43'	;２ｃｈ入力パルス幅（前進・後進）
	PIN3C	EQU	H'44'	;３ｃｈ入力パルス幅（砲身上下用）
	PIN4C	EQU	H'45'	;４ｃｈ入力パルス幅（砲塔旋回用）
	GAP1	EQU	H'46'	;赤外線通信ＧＡＰカウント用
	GAP2	EQU	H'47'
	L_ONOFF	EQU	H'48'	;ライトON/OFF
	L_ONOFFT	EQU	H'49'	;ライトON/OFF許可

	ORG	0
	GOTO	START
;------------------------減速ＤＡＴＡ---------------------
	ORG	3
SSOKUDO
	ADDWF	PCL,1	;X.Y
	RETLW	4	;0.0
	RETLW	3	;0.1
	RETLW	2	;0.2
	RETLW	2	;0.3
	RETLW	1	;0.4
	RETLW	1	;0.5
	RETLW	0	;0.6
	RETLW	0	;0.7
	RETLW	0	;0.8
	RETLW	0	;0.9
	RETLW	6	;1.0
	RETLW	5	;1.1
	RETLW	4	;1.2
	RETLW	3	;1.3
	RETLW	2	;1.4
	RETLW	2	;1.5
	RETLW	0	;1.6
	RETLW	0	;1.7
	RETLW	0	;1.8
	RETLW	0	;1.9
	RETLW	9	;2.0
	RETLW	8	;2.1
	RETLW	7	;2.2
	RETLW	6	;2.3
	RETLW	5	;2.4
	RETLW	2	;2.5
	RETLW	0	;2.6
	RETLW	0	;2.7
	RETLW	0	;2.8
	RETLW	0	;2.9
	RETLW	15	;3.0
	RETLW	14	;3.1
	RETLW	12	;3.2
	RETLW	10	;3.3
	RETLW	8	;3.4
	RETLW	3	;3.5
	RETLW	0	;3.6
	RETLW	0	;3.7
	RETLW	0	;3.8
	RETLW	0	;3.9
	RETLW	20	;4.0
	RETLW	17	;4.1
	RETLW	14	;4.2
	RETLW	12	;4.3
	RETLW	9	;4.4
	RETLW	4	;4.5
	RETLW	0	;4.6
	RETLW	0	;4.7
	RETLW	0	;4.8
	RETLW	0	;4.9
	RETLW	25	;5.0
	RETLW	22	;5.1
	RETLW	18	;5.2
	RETLW	14	;5.3
	RETLW	10	;5.4
	RETLW	5	;5.5
	RETLW	0	;5.6
	RETLW	0	;5.7
	RETLW	0	;5.8
	RETLW	0	;5.9
	RETLW	30	;6.0
	RETLW	26	;6.1
	RETLW	22	;6.2
	RETLW	18	;6.3
	RETLW	13	;6.4
	RETLW	6	;6.5
	RETLW	0	;6.6
	RETLW	0	;6.7
	RETLW	0	;6.8
	RETLW	0	;6.9
	RETLW	35	;7.0
	RETLW	30	;7.1
	RETLW	24	;7.2
	RETLW	19	;7.3
	RETLW	14	;7.4
	RETLW	6	;7.5
	RETLW	0	;7.6
	RETLW	0	;7.7
	RETLW	0	;7.8
	RETLW	0	;7.9
	RETLW	40	;8.0
	RETLW	34	;8.1
	RETLW	28	;8.2
	RETLW	22	;8.3
	RETLW	16	;8.4
	RETLW	7	;8.5
	RETLW	0	;8.6
	RETLW	0	;8.7
	RETLW	0	;8.8
	RETLW	0	;8.9
	RETLW	45	;9.0
	RETLW	40	;9.1
	RETLW	32	;9.2
	RETLW	24	;9.3
	RETLW	18	;9.4
	RETLW	10	;9.5
	RETLW	0	;9.6
	RETLW	0	;9.7
	RETLW	0	;9.8
	RETLW	0	;9.9
	RETLW	42	;9.0
	RETLW	36	;9.1
	RETLW	30	;9.2
	RETLW	24	;9.3
	RETLW	18	;9.4
	RETLW	8	;9.5
	RETLW	0	;9.6
	RETLW	0	;9.7
	RETLW	0	;9.8
	RETLW	0	;9.9
	RETLW	42	;9.0
	RETLW	36	;9.1
	RETLW	30	;9.2
	RETLW	24	;9.3
	RETLW	18	;9.4
	RETLW	12	;9.5
	RETLW	5	;9.6
	RETLW	0	;9.7
	RETLW	0	;9.8
	RETLW	0	;9.9
;------------------------　片側減速時の反対側補正値　-------
SSOKUDO2
	ADDWF	PCL,1	;X
	RETLW	0	;0
	RETLW	1	;1
	RETLW	2	;2
	RETLW	3	;3
	RETLW	4	;4
	RETLW	5	;5
	RETLW	6	;6
	RETLW	7	;7
	RETLW	8	;8
	RETLW	0	;9
	RETLW	0	;0

;=================================================================
START
	BSF	STATUS,RP0	;バンク１に切替
	MOVLW	07H		;
	MOVWF	ADCON1		;A/D変換設定を全てディジタルモードに
	MOVLW	B'001111'	;ポートAの入出力モード設定
	MOVWF	TRISA		;
	CLRF	TRISB		;ポートＢ全て出力モード設定
	CLRF	TRISC		;ポートＣ全て出力モード設定

	BCF	STATUS,RP0	;バンク０に切替
	CLRF	PORTB		;ポートＢクリア
	CLRF	PORTC		;ポートCクリア
;---------------　初期値の設定　-----------------
	MOVLW	150		;砲塔旋回値を１５０（停止）に設定
	MOVWF	CENTER3
	MOVWF	POUT3	
	MOVLW	150		;アンプのセンターを１５０（停止）に設定
	MOVWF	CENTER1
	MOVLW	150		;アンプのセンターを１５０（停止）に設定
	MOVWF	CENTER2
;--------------音・発光の初期値 ----------------
	BSF	PORTC,0		;音のスイッチ off
        MOVLW	B'110000'	;RA4・5のLEDをOFF
	MOVWF	PORTA		;

;-----------リコイル用サーボ・砲身上下用サーボの停止位置 ------------
	MOVLW	150		;スイッチを入れたときの砲身のデフォルト角度設定
	MOVWF	POUT4		;
	MOVLW	200		;リコイル用サーボの停止位置
	MOVWF	POUT5
	MOVLW	255		;
	MOVWF	COUNT8
	CALL	SERVO1		;サーボパルス出力
	CALL	SERVO2		;サーボパルス出力
	MOVLW	2		;砲身上下のスピード0-5
	MOVWF	HSPEED		;

;-------------------メインルーチン -------------
LOOP1
	CALL	PULSIN1A		;1chパルス幅取得
;	CALL	PULSIN2A		;2chパルス幅取得
	CALL	PULSIN40		;4chパルス幅取得

	MOVF	PIN2,0
	MOVWF	POUT1		;
	MOVWF	POUT2		;
	CALL	TIHOCOUNT	;大砲装填時間の計算

;--------停止中　148=< PIN2 >=152 の範囲

	MOVLW	152		;前後スティックが153以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=152はSTEP51へ
	GOTO	STEP51
	MOVLW	148		;前後スティックが148以上だったら停止
	SUBWF	PIN2,0		;
	BTFSS	STATUS,0	;(CY=0) PIN2<148はSTEP51へ
	GOTO	STEP51
	MOVF	CENTER1,0	;W=CENTER1
	MOVWF	POUT1
	MOVF	CENTER2,0	;W=CENTER2
	MOVWF	POUT2

	CALL	PULSIN30	;3chパルス幅取得(停止中のみ3chを入力する)

	CALL	HOTO_S
	CALL	M_TIM		;モーターＯＮ
	CALL	MOT_01		;モーターＯＦＦ
	CALL	M_TIM		;モーターＯＮ
	CALL	MOT_01		;モーターＯＦＦ
	CALL	M_TIM		;モーターＯＮ
	CALL	MOT_01		;モーターＯＦＦ

	CALL	SERVO1		;サーボパルス出力
	CALL	SERVO2		;サーボパルス出力

;--------停止中の発砲音　115> PIN1 OR PIN1>=190 の範囲

	MOVLW	180		;
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=180は機銃音へ
	GOTO	STEP_A01

	MOVLW	120		;
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<120は大砲音へ
	GOTO	STEP_A02

	BSF	PORTC,0		;音のスイッチ off
	MOVLW	B'110000'	;(0はそのまま1は1に)
	IORWF	PORTA,1		;RA4・5のLEDをOFF

	GOTO	STEP_L01
;--------------------機銃音と発光
STEP_A01
	BSF	STATUS,RP0	;Change Page1
	BSF	TRISC,1		;RB6を入力に切替
	BCF	STATUS,RP0	;Change Page0
	CALL	TIM20		;ちょっと待つ
	BCF	PORTC,0		;音のＳＷ on(機関銃音)

	MOVLW	B'010000'	;(1は反転、0はそのまま)
	XORWF	PORTA,1		;RA4のLEDをON/OFF

	CALL	PULSOFF		;発光の間隔の時間稼ぎ
	CALL	PULSOFF
	CALL	PULSOFF		;発光の間隔の時間稼ぎ
	CALL	PULSOFF
	CALL	PULSOFF		;発光の間隔の時間稼ぎ
	CALL	PULSOFF

	GOTO	LOOP1

;--------------------大砲音と発光
STEP_A02

	MOVLW	255		;COUNT4<>255だったら
	SUBWF	COUNT8,0	;
	BTFSS	STATUS,2	;(Z=0) COUNT4<>255
	GOTO	LOOP1

	MOVLW	B'011111'	;(1はそのまま0は0に)
	ANDWF	PORTA,1		;RA5のLEDをON

	BSF	STATUS,RP0	;Change Page1
	BCF	TRISC,1		;RC1を出力に切替
	BCF	STATUS,RP0	;Change Page0
	CALL	TIM20		;ちょっと待つ
	BSF	PORTC,1		;RC1 on（音切り換え）
	CALL	TIM20		;ちょっと待つ
	BCF	PORTC,0		;音のＳＷ on(大砲音)

	MOVLW	90		;砲身後退サーボの値を９０にして
	MOVWF	POUT5		;砲身を一挙に後退

;---------------　発射時の振動---------------------------
	MOVLW	100
	MOVWF	POUT1
	MOVWF	POUT2
	CALL	M_TIM		;モーターＯＮ

	CALL	SERVO2		;サーボパルス出力
	MOVLW	255
	CALL	TIM25
	MOVLW	0
	MOVWF	COUNT12		;振動停止のカウンター
;-------------------------------------------------------------------
	MOVLW	160		;リコイルタイマーセット(砲身後退時間)
	MOVWF	COUNT4
STEP_A04
	CALL	SERVO2		;サーボパルス出力

	MOVLW	255
	CALL	TIM25
;----------------------------  振動　-----------------
	INCF	COUNT12,1	;
	MOVLW	10
	SUBWF	COUNT12,0
	BTFSC	STATUS,2	;Z=1 で
	GOTO	STEP_A05	; COUNT12=8(バックの振動停止)
	GOTO	STEP_A06	; COUNT12<>8
STEP_A05			;-------------停止-----------
	MOVLW	150
	MOVWF	POUT1
	MOVWF	POUT2
	CALL	M_TIM		;モーターOFF
	GOTO	STEP_A09
STEP_A06			;------------前進-----------
	MOVLW	30
	SUBWF	COUNT12,0
	BTFSC	STATUS,2	;Z=1 で
	GOTO	STEP_A07	; COUNT12=15(前進の振動)
	GOTO	STEP_A08	; COUNT12<>15
STEP_A07
	MOVLW	200
	MOVWF	POUT1
	MOVWF	POUT2
	CALL	M_TIM		;モーターON
STEP_A08			;------------停止----------
	MOVLW	42
	SUBWF	COUNT12,0
	BTFSC	STATUS,2	;Z=1 で
	GOTO	STEP_A05	; COUNT20=15(前進の振動停止)
STEP_A09
;-------------------------------------------------------------------
	DECFSZ	COUNT4,1		;COUNT4=COUNT4-1
	GOTO	STEP_A04

	MOVLW	200		;砲身後退サーボの値を200に戻す
	MOVWF	POUT5		;

	MOVLW	B'100000'	;(0はそのまま1は1に)
	IORWF	PORTA,1		;RA5のLEDをOFF

	MOVLW	80		;しばらく撃てなくするタイマーをセット
	MOVWF	COUNT8

	GOTO	LOOP1
;--------------------大砲音発射カウント戻す　SUB
TIHOCOUNT
;-----------大砲の発射準備? COUNT8<>255は撃てない
	MOVLW	250		;250以上だったら
	SUBWF	COUNT8,0	;
	BTFSS	STATUS,0	;(CY=0) COUNT8<250は
	GOTO	TIHOCOUNT02
	MOVLW	255		;
	MOVWF	COUNT8		;COUNT8=255
	RETURN

TIHOCOUNT02
	MOVLW	2		;大砲装填時間(1-5)
	ADDWF	COUNT8,1	;COUNT8=COUNT8+2
	RETURN

;-----------ライトの点灯
STEP_L01

	MOVLW	110		;
	SUBWF	PIN3,0		;
	BTFSC	STATUS,0	;(CY=1) PIN3>=110は砲身上下へ
	GOTO	STEP_C01


	MOVLW	1
	SUBWF	L_ONOFFT,0
	BTFSS	STATUS,2	;(Z=0) L_ONOFF<>1はジャンプ
 	GOTO	LOOP1

	MOVLW	1
	SUBWF	L_ONOFF,0
	BTFSS	STATUS,2	;(Z=0) L_ONOFF<>1はジャンプ
	GOTO	STEP_L02

;	BSF	PORTC,7		;RC7 off　ライト
	MOVLW	0		;L_ONOFF=0
	MOVWF	L_ONOFF
	CLRF	L_ONOFFT
 	GOTO	LOOP1
STEP_L02
;	BCF	PORTC,7		;RC7 on ライト
	MOVLW	1		;L_ONOFF=1
	MOVWF	L_ONOFF
	CLRF	L_ONOFFT
 	GOTO	LOOP1

;--------停止中の砲身上下　120>PIN3 or PIN3>=180 の範囲
STEP_C01
	MOVLW	180		;
	SUBWF	PIN3,0		;
	BTFSC	STATUS,0	;(CY=1) PIN3>=180は砲身上へ
	GOTO	STEP_C10

	MOVLW	120		;
	SUBWF	PIN3,0		;
	BTFSS	STATUS,0	;(CY=0) PIN3<120は砲身下へ
	GOTO	STEP_C20

	MOVLW	1		;L_ONOFFT=1	ライトスイッチ許可
	MOVWF	L_ONOFFT

 	GOTO	LOOP1

STEP_C10

	INCF	POUT4,1		;POUT4=POUT4+1

	MOVLW	200		;
	SUBWF	POUT4,0		;
	BTFSS	STATUS,0	;(CY=0) POUT4<200はSTEP_C30へ
	GOTO	STEP_C30

	MOVLW	200		;砲身上下サーボを200に固定
	MOVWF	POUT4		;POUT4=200
	GOTO	STEP_C30

STEP_C20

	DECF	POUT4,1		;POUT4=POUT4-1

	MOVLW	100		;
	SUBWF	POUT4,0		;
	BTFSC	STATUS,0	;(CY=1) POUT4>=100はSTEP_C30へ
	GOTO	STEP_C30

	MOVLW	100		;砲身上下サーボを100に固定
	MOVWF	POUT4		;POUT4=100
STEP_C30

 	GOTO	LOOP1
;=================================================
;---------------  走行ルーチン--------------------
STEP51
;--------超信地旋回左
	MOVLW	115		;左右スティックが115以下だったら
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=115はSTEP2へ
	GOTO	STEP2		;
	MOVLW	210		;
	MOVWF	POUT1		;逆転速度＝210-（速度－90）
	MOVLW	90		;
	SUBWF	PIN2,0		;(W=PIN2-90)
	SUBWF	POUT1,1		;(POUT1＝210-W)
	GOTO	PULSOUT
STEP2
;--------超信地旋回右
	MOVLW	185		;左右スティックが185以上だったら
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<185はSTEP3へ
	GOTO	STEP3		;
	MOVLW	210		;
	MOVWF	POUT2		;逆転速度＝210-（速度－90）
	MOVLW	90		;
	SUBWF	PIN2,0		;(W=PIN2-90)
	SUBWF	POUT2,1		;(POUT2＝210-W)
	GOTO	PULSOUT
STEP3

;--------左旋回
	MOVLW	148		;左右スティックが147以下だったら
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=148はSTEP7へ
	GOTO	STEP7		;
;--------前進
	MOVLW	150		;前後スティックが150以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=150はSTEP62へ
	GOTO	STEP62		;

	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
	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

STEP62
;--------後進
	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
	ADDWF	POUT1,1		;POUT1=POUT1+W

	MOVF	SOKUDO,0	;W=SOKUDO	速度補正
	ADDWF	POUT2,1		;POUT2=POUT2+W

	GOTO	PULSOUT

STEP7

;--------右旋回
	MOVLW	153		;左右スティックが153以上だったら
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<153はSTEP8へ
	GOTO	STEP8		;

;--------前進
	MOVLW	150		;前後スティックが150以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=150はSTEP72へ
	GOTO	STEP72		;

	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
	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

STEP72
;--------後進
	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
	ADDWF	POUT2,1		;POUT2=POUT2+W

	MOVF	SOKUDO,0	;W=SOKUDO	速度補正
	ADDWF	POUT1,1		;POUT1=POUT1+W
	GOTO	PULSOUT
STEP8
PULSOUT
	CALL	HOTO_S
;--------- 走行用パルス出力
	CALL	M_TIM		;モーターＯＮ
	CALL	MOT_01		;モーターＯＦＦ
	CALL	M_TIM		;モーターＯＮ
	CALL	MOT_01		;モーターＯＦＦ
	CALL	M_TIM		;モーターＯＮ
	CALL	MOT_01		;モーターＯＦＦ
	CALL	M_TIM		;モーターＯＮ
	CALL	MOT_01		;モーターＯＦＦ
	CALL	M_TIM		;モーターＯＮ
	CALL	MOT_01		;モーターＯＦＦ

	GOTO	LOOP1

;--------- 砲塔旋回
HOTO_S
	MOVLW	118		;
	SUBWF	PIN4,0		;
	BTFSC	STATUS,0	;(CY=1) PIN4>=120はNEXT52へ
	GOTO	NEXT52		;

	MOVLW	40		;
	ADDWF	CENTER3,0	;POUT3=CENTER3+40
	MOVWF	POUT3		;
	GOTO	NEXT53		;

NEXT52
	MOVLW	135		;
	SUBWF	PIN4,0		;
	BTFSC	STATUS,0	;(CY=1) PIN4>=130はNEXT52Bへ
	GOTO	NEXT52B		;

	MOVLW	30		;
	ADDWF	CENTER3,0	;POUT3=CENTER3+30
	MOVWF	POUT3		;
	GOTO	NEXT53		;
NEXT52B
	MOVLW	182		;左右旋回スティックが180以上だったら
	SUBWF	PIN4,0		;
	BTFSS	STATUS,0	;(CY=0) PIN4<180はNEXT52Cへ
	GOTO	NEXT52C		;

	MOVLW	40		;
	SUBWF	CENTER3,0	;POUT3=CENTER3-40
	MOVWF	POUT3		;
	GOTO	NEXT53		;

NEXT52C
	MOVLW	165		;左右旋回スティックが170以上だったら
	SUBWF	PIN4,0		;
	BTFSS	STATUS,0	;(CY=0) PIN4<180はNEXT52Dへ
	GOTO	NEXT52D		;

	MOVLW	30		;
	SUBWF	CENTER3,0	;POUT3=CENTER3-30
	MOVWF	POUT3		;
	GOTO	NEXT53		;

NEXT52D

	MOVF	CENTER3,0	;W=CENTER3
	MOVWF	POUT3		;--------砲塔旋回停止

NEXT53
	RETURN


;-------------------回転時間の計算（149-151は停止） -------------
M_TIM
;-----1ch
	MOVLW	151		;1chが151以上はジャンプ
	SUBWF	POUT1,0		;
	BTFSC	STATUS,0	;(CY=1) POUT1>=151はM_TIM10へ
	GOTO	M_TIM10

	MOVLW	150		;1chが150未満はジャンプ
	SUBWF	POUT1,0		;
	BTFSS	STATUS,0	;(CY=0) POUT1<150はM_TIM12へ
	GOTO	M_TIM12

	MOVLW	0		;1ch 停止
	MOVWF	PIN1B

	BCF	PORTB,0		;off
	BCF	PORTB,1		;off

	GOTO	M_TIM14
; -------正転---------
M_TIM10
	MOVLW	150		;W=150
	SUBWF	POUT1,0		;W=POUT1-150
	MOVWF	PIN1B

	BCF	PORTB,1		;off
	BSF	PORTB,2		;on
	BSF	PORTB,0		;on
	BCF	PORTB,3		;off

	GOTO	M_TIM14

; -------逆転---------
M_TIM12
	MOVF	POUT1,0		;W=POUT1
	SUBLW	150		;W=150-速度
	MOVWF	PIN1B

	BCF	PORTB,0		;off
	BSF	PORTB,3		;on
	BSF	PORTB,1		;on
	BCF	PORTB,2		;off


M_TIM14

;-----2ch
	MOVLW	151		;2chが151以上はジャンプ
	SUBWF	POUT2,0		;
	BTFSC	STATUS,0	;(CY=1) POUT2>=151はM_TIM20へ
	GOTO	M_TIM20

	MOVLW	150		;2chが150未満はジャンプ
	SUBWF	POUT2,0		;
	BTFSS	STATUS,0	;(CY=0) POUT2<150はM_TIM22へ
	GOTO	M_TIM22

	MOVLW	0		;2ch 停止
	MOVWF	PIN2B

	BCF	PORTB,4		;off
	BCF	PORTB,5		;off

	GOTO	M_TIM24
M_TIM20
	MOVLW	150		;W=150
	SUBWF	POUT2,0		;W=POUT2-150
	MOVWF	PIN2B

	BCF	PORTB,5		;off
	BSF	PORTB,6		;on
	BSF	PORTB,4		;on
	BCF	PORTB,7		;off

	GOTO	M_TIM24

M_TIM22
	MOVF	POUT2,0		;W=POUT2
	SUBLW	150		;W=150-速度
	MOVWF	PIN2B

	BCF	PORTB,4		;off
	BSF	PORTB,7		;on
	BSF	PORTB,5		;on
	BCF	PORTB,6		;off

M_TIM24

;-----3ch
	MOVLW	151		;3chが151以上はジャンプ
	SUBWF	POUT3,0		;
	BTFSC	STATUS,0	;(CY=1) POUT3>=151はM_TIM30へ
	GOTO	M_TIM30

	MOVLW	150		;3chが150未満はジャンプ
	SUBWF	POUT3,0		;
	BTFSS	STATUS,0	;(CY=0) POUT3<150はM_TIM32へ
	GOTO	M_TIM32

	MOVLW	0		;3ch 停止
	MOVWF	PIN3B

	BCF	PORTC,7		;off
	BCF	PORTC,6		;off

	GOTO	M_TIM34
M_TIM30
	MOVLW	150		;W=150
	SUBWF	POUT3,0		;W=POUT3-150
	MOVWF	PIN3B

	BCF	PORTC,6		;off	1
	BSF	PORTC,5		;on	2
	BSF	PORTC,7		;on	0
	BCF	PORTC,4		;off	3

	GOTO	M_TIM34

M_TIM32
	MOVF	POUT3,0		;W=POUT3
	SUBLW	150		;W=150-速度
	MOVWF	PIN3B

	BCF	PORTC,7		;off	0
	BSF	PORTC,4		;on	3
	BSF	PORTC,6		;on	1
	BCF	PORTC,5		;off	2

M_TIM34
	MOVLW	100
	CALL	TIM25		;

	RETURN

;------------------- モーター可動時間 -------------
MOT_01
	MOVLW	40		;タイマーセット
	MOVWF	TIME1

MOT_02
	MOVLW	10
	CALL	TIM25		;


	INCF	PIN1B,1		;PIN1B=PIN1B+1
	DECFSZ	PIN1B,1		;PIN1B=PIN1B-1
	GOTO	MOT_10		;PIN1B>0 で　MOT_10
	GOTO	MOT_12		;PIN1B=0 で　MOT_12
MOT_10
	DECFSZ	PIN1B,1		;PIN1B=PIN1B-1
	GOTO	MOT_12		;PIN1Bが0になるまで繰り返し

	BCF	PORTB,0		;off
	BCF	PORTB,1		;off

MOT_12

	INCF	PIN2B,1		;PIN2B=PIN2B+1
	DECFSZ	PIN2B,1		;PIN2B=PIN2B-1
	GOTO	MOT_20		;PIN2B>0 で　MOT_20
	GOTO	MOT_14		;PIN2B=0 で　MOT_14
MOT_20
	DECFSZ	PIN2B,1		;PIN2B=PIN2B-1
	GOTO	MOT_14		;PIN2Bが0になるまで繰り返し

	BCF	PORTB,4		;off
	BCF	PORTB,5		;off

MOT_14

	INCF	PIN3B,1		;PIN3B=PIN3B+1
	DECFSZ	PIN3B,1		;PIN3B=PIN3B-1
	GOTO	MOT_16		;PIN3B>0 で　MOT_16
	GOTO	MOT_22		;PIN3B=0 で　MOT_22
MOT_16
	DECFSZ	PIN3B,1		;PIN3B=PIN3B-1
	GOTO	MOT_22		;PIN3Bが0になるまで繰り返し

	BCF	PORTC,7		;off
	BCF	PORTC,6		;off

MOT_22

	DECFSZ	TIME1,1		;TIME1=TIME1-1
	GOTO	MOT_02		;TIME1が0になるまで繰り返し

	RETURN
;-----------------------サーボ信号出力砲身上下------------------
SERVO1
	BSF	PORTC,2		;RC2 on
	MOVF	POUT4,0
	CALL	TIM25
	BCF	PORTC,2		;RC2 off
	RETURN

;-----------------------サーボ信号出力リコイル------------------
SERVO2
	BSF	PORTC,3		;RC3 on
	MOVF	POUT5,0
	CALL	TIM25
	BCF	PORTC,3		;RC3 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
	GOTO	$+1	;+2=18
	GOTO	$+1	;+2=20
	GOTO	$+1	;+2=22
	GOTO	$+1	;+2=24
	GOTO	$+1	;+2=26
	GOTO	$+1	;+2=28
	GOTO	$+1	;+2=30
	GOTO	$+1	;+2=32
	GOTO	$+1	;+2=34
	GOTO	$+1	;+2=36
	GOTO	$+1	;+2=38
	GOTO	$+1	;+2=40
	GOTO	$+1	;+2=42
	RETURN		;+2=44

;-------------  Ｗレジスタ分のカウントタイマー(1カウント=25サイクル)×Ｗ
TIM25
	MOVWF	COUNT1		;1+1=2
TIM26
	CALL	TIM20		;44+2=46
	NOP			;+1=47
	DECFSZ	COUNT1,1	;+1=48
	GOTO	TIM26		;+2=50
	RETURN			;+1+(W*50)+2-1

;-----------パルスとパルスの間の空き時間-----------------
PULSOFF
	MOVLW	255
	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			;

;---------------------------- パルス測定--------------------
;-----------3CHパルス幅測定（PIN3）------
PULSIN3A
	BTFSS	PORTA,2		;OFF の時
	GOTO	PULSIN30		;入力開始
	CALL	TIM20
	GOTO	PULSIN3A	;次の信号まで待つ
;-----------3CHパルス幅測定（PIN3)-----------
PULSIN30
	BTFSS	PORTA,2		;OFF の時
	GOTO	PULSIN30

	CLRF	PIN3		;カウンター初期化
PULSIN31
	CALL	TIM20		;20サイクル待ち	(44+2=46)
	INCF	PIN3,1		;PIN3=PIN3+1	(47)
	BTFSC	PORTA,2		;ＯＮの時は	(48)
	GOTO	PULSIN31	;カウント継続	(50)
;----------DATAチェック
	MOVLW	90		;カウントが90以下だったら
	SUBWF	PIN3,0		;カウントしなおし！
	BTFSS	STATUS,0	;(CY=0) PIN3<90
	GOTO	PULSIN3A		;

	RETURN

;-----------1CHパルス幅測定（PIN1）前進・後進のスティックの信号チェック-------
PULSIN1A
	BTFSS	PORTA,0		;OFF の時
	GOTO	PULSIN10		;入力開始
	CALL	TIM20
	GOTO	PULSIN1A	;次の信号まで待つ
;-----------1CHパルス幅測定（PIN1）左右のステック-----------
PULSIN10

	BTFSS	PORTA,0		;OFF の時
	GOTO	PULSIN10		;入力開始
	CLRF	PIN1		;カウンター初期化
PULSIN11
	CALL	TIM20		;20サイクル待ち	(44+2=46)
	INCF	PIN1,1		;PIN1=PIN1+1	(47)
	BTFSC	PORTA,0		;ＯＮの時は	(48)
	GOTO	PULSIN11	;カウント継続	(50)

	MOVLW	90		;カウントが90以下だったら
	SUBWF	PIN1,0		;カウントしなおし！
	BTFSS	STATUS,0	;(CY=0) PIN1<90
	GOTO	PULSIN1A		;
	GOTO	PULSIN20		;
;-----------2CHパルス幅測定（PIN2）------
PULSIN2A
	BTFSS	PORTA,1		;OFF の時
	GOTO	PULSIN20		;入力開始
	CALL	TIM20
	GOTO	PULSIN2A	;次の信号まで待つ
;-----------2CHパルス幅測定（PIN2)-----------
PULSIN20
	BTFSS	PORTA,1		;OFF の時
	GOTO	PULSIN20

	CLRF	PIN2		;カウンター初期化
PULSIN21
	CALL	TIM20		;20サイクル待ち	(44+2=46)
	INCF	PIN2,1		;PIN2=PIN2+1	(47)
	BTFSC	PORTA,1		;ＯＮの時は	(48)
	GOTO	PULSIN21	;カウント継続	(50)

	MOVLW	90		;カウントが90以下だったら
	SUBWF	PIN2,0		;カウントしなおし！
	BTFSS	STATUS,0	;(CY=0) PIN2<90
	GOTO	PULSIN2A		;
	RETURN

;-----------4CHパルス幅測定（PIN4）------
PULSIN4A
	BTFSS	PORTA,3		;OFF の時
	GOTO	PULSIN40	;入力開始
	CALL	TIM20
	GOTO	PULSIN4A	;次の信号まで待つ
;-----------4CHパルス幅測定（PIN4)-----------
PULSIN40
	BTFSS	PORTA,3		;OFF の時
	GOTO	PULSIN40
	CLRF	PIN4		;カウンター初期化
PULSIN41
	CALL	TIM20		;20サイクル待ち	(44+2=46)
	INCF	PIN4,1		;PIN4=PIN4+1	(47)
	BTFSC	PORTA,3		;ＯＮの時は	(48)
	GOTO	PULSIN41	;カウント継続	(50)
;----------DATAチェック
	MOVLW	90		;カウントが90以下だったら
	SUBWF	PIN4,0		;カウントしなおし！
	BTFSS	STATUS,0	;(CY=0) PIN4<90
	GOTO	PULSIN4A	;
	RETURN

	END





