;２サーボ（アンプ）コントロールプログラム
;Ver1.0.0	2000.06.28	By T.HINOKI

		LIST		P=16C84,R=DEC
	STATUS	EQU	3
	PG	EQU	5
	PORTA	EQU	5
	PORTB	EQU	6
	TRISA	EQU	H'85'
	TRISB	EQU	H'86'
	COUNT1	EQU	H'10'
	COUNT2	EQU	H'11'
	PIN1	EQU	H'12'	;1ｃｈ入力パルス幅（左・右）
	PIN2	EQU	H'13'	;２ｃｈ入力パルス幅（前進・後進）
	POUT1	EQU	H'14'	;1ｃｈ出力パルス幅（左キャタ速度）
	POUT2	EQU	H'15'	;２ｃｈ出力パルス幅（右キャタ速度）
	KEISAN1	EQU	H'16'	;計算用変数
	KEISAN2	EQU	H'17'	;計算用変数
	KEISAN3	EQU	H'18'	;計算用変数


	CLRF	PORTB		;クリア PORTB
	BSF	STATUS,PG	;Change Page1
	MOVLW	B'1111'		;TRISA=F  All Input
	MOVWF	TRISA
	CLRF	TRISB		;TRISB=0  All output
	BCF	STATUS,PG	;Change Page0
LOOP1

	CALL	PULSIN1		;1chパルス幅取得
	CALL	PULSIN2		;2chパルス幅取得

	MOVF	PIN2,0
	MOVWF	POUT1
	MOVWF	POUT2

;--------超信地旋回左
	MOVLW	106		;左右スティックが107以下だったら
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=106はNEXT2へ
	GOTO	NEXT2		;
	MOVLW	210		;
	MOVWF	POUT1		;逆転速度＝210-（速度－90）
	MOVLW	90		;
	SUBWF	PIN2,0		;(W=PIN2-90)
	SUBWF	POUT1,1		;(POUT1＝210-W)
	GOTO	PULSOUT
NEXT2
;--------超信地旋回右
	MOVLW	194		;左右スティックが194以上だったら
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<194はNEXT3へ
	GOTO	NEXT3		;
	MOVLW	210		;
	MOVWF	POUT2		;逆転速度＝210-（速度－90）
	MOVLW	90		;
	SUBWF	PIN2,0		;(W=PIN2-90)
	SUBWF	POUT2,1		;(POUT2＝210-W)
	GOTO	PULSOUT
NEXT3
;--------信地旋回左
	MOVLW	114		;左右スティックが115以下だったら
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=114はNEXT4へ
	GOTO	NEXT4		;
	MOVLW	150		;
	MOVWF	POUT1		;速度＝150(片側停止)
	GOTO	PULSOUT
NEXT4
;--------信地旋回右
	MOVLW	185		;左右スティックが185以上だったら
	SUBWF	PIN1,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<185はNEXT5へ
	GOTO	NEXT5		;
	MOVLW	150		;
	MOVWF	POUT2		;速度＝150(片側停止)
	GOTO	PULSOUT
NEXT5

;--------停止中
	MOVLW	154		;前後スティックが154以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=154はNEXT51へ
	GOTO	NEXT51		;
	MOVLW	147		;前後スティックが147以上だったら停止
	SUBWF	PIN2,0		;
	BTFSS	STATUS,0	;(CY=0) PIN1<147はNEXT51へ
	GOTO	NEXT51		;
	MOVLW	150		;左右中間(停止)に
	MOVWF	POUT1
	MOVWF	POUT2
	GOTO	PULSOUT
NEXT51


;--------左旋回
	MOVLW	151		;左右スティックが150以下だったら
	SUBWF	PIN1,0		;
	BTFSC	STATUS,0	;(CY=1) PIN1>=151はNEXT7へ
	GOTO	NEXT7		;
;--------前進
	MOVLW	150		;前後スティックが150以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=150はNEXT62へ
	GOTO	NEXT62		;

	MOVF	POUT1,0
	SUBLW	150		;150-速度
	CALL	WARU6		;(150-速度)÷6
	DECF	KEISAN2,0	;W=KEISAN2-1
	MOVWF	KEISAN3		;KEISAN3=W
	MOVF	PIN1,0
	SUBLW	150		;150-角度
	CALL	WARU5		;(150-角度)÷5

NEXT61
	MOVF	KEISAN3,0	;W=KEISAN3
	ADDWF	POUT1,1		;POUT1=POUT1+W
	DECFSZ	KEISAN2,1	;KEISAN2=KEISAN2-1
	GOTO	NEXT61		;KEISAN2が0になるまで繰り返し

	GOTO	PULSOUT

NEXT62
;--------後進
	MOVLW	150		;W=150
	SUBWF	POUT1,0		;速度-150
	CALL	WARU6		;(速度-150)÷6
	DECF	KEISAN2,0	;W=KEISAN2-1
	MOVWF	KEISAN3		;KEISAN3=W
	MOVF	PIN1,0
	SUBLW	150		;150-角度
	CALL	WARU5		;(150-角度)÷5

NEXT63
	MOVF	KEISAN3,0	;W=KEISAN3
	SUBWF	POUT1,1		;POUT1=POUT1-W
	DECFSZ	KEISAN2,1	;KEISAN2=KEISAN2-1
	GOTO	NEXT63		;KEISAN2が0になるまで繰り返し

	GOTO	PULSOUT

NEXT7

;--------右旋回
;--------前進
	MOVLW	150		;前後スティックが150以下だったら
	SUBWF	PIN2,0		;
	BTFSC	STATUS,0	;(CY=1) PIN2>=150はNEXT72へ
	GOTO	NEXT72		;

	MOVF	POUT2,0
	SUBLW	150		;150-速度
	CALL	WARU6		;(150-速度)÷6
	DECF	KEISAN2,0	;W=KEISAN2-1
	MOVWF	KEISAN3		;KEISAN3=W


	MOVLW	150
	SUBWF	PIN1,0		;角度-150
	CALL	WARU5		;(角度-150)÷5

NEXT71
	MOVF	KEISAN3,0	;W=KEISAN3
	ADDWF	POUT2,1		;POUT2=POUT2+W
	DECFSZ	KEISAN2,1	;KEISAN2=KEISAN2-1
	GOTO	NEXT71		;KEISAN2が0になるまで繰り返し

	GOTO	PULSOUT

NEXT72
;--------後進
	MOVLW	150		;W=150
	SUBWF	POUT1,0		;速度-150
	CALL	WARU6		;(速度-150)÷6
	DECF	KEISAN2,0	;W=KEISAN2-1
	MOVWF	KEISAN3		;KEISAN3=W
	MOVF	PIN1,0

	MOVLW	150		;W=150
	SUBWF	PIN1,0		;角度-150
	CALL	WARU5		;(角度-150)÷5

NEXT73
	MOVF	KEISAN3,0	;W=KEISAN3
	SUBWF	POUT2,1		;POUT2=POUT2-W
	DECFSZ	KEISAN2,1	;KEISAN2=KEISAN2-1
	GOTO	NEXT73		;KEISAN2が0になるまで繰り返し

	GOTO	PULSOUT

NEXT8

PULSOUT
	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

	CALL	PULSOFF

	GOTO	LOOP1

;***********　Ｗ÷５＝Ｗ
WARU5
	MOVWF	KEISAN1		;ＷをKEISAN1に代入
	MOVLW	0		;
	MOVWF	KEISAN2		;カウンター初期化
WARU51
	MOVLW	5		;
	SUBWF	KEISAN1,1
	BTFSS	STATUS,0	;CY=0　で終了
	RETURN
	INCF	KEISAN2,1	;KEISAN2=KEISAN2+1
	GOTO	WARU51

;***********　Ｗ÷６＝Ｗ
WARU6
	MOVWF	KEISAN1		;ＷをKEISAN1に代入
	MOVLW	0		;
	MOVWF	KEISAN2		;カウンター初期化
WARU61
	MOVLW	6		;
	SUBWF	KEISAN1,1
	BTFSS	STATUS,0	;CY=0　で終了
	RETURN
	INCF	KEISAN2,1	;KEISAN2=KEISAN2+1
	GOTO	WARU61


;-------------  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	255
	CALL	TIM25		;
	MOVLW	255
	CALL	TIM25		;
	RETURN

;-----------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）前進・後進のスティック-----------
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

	END
