;**************************************************
;  模型のスケール速度を測定する
; 16F648A 外部クロック(水晶発振器 4MHｚ)使用
; Programmed by Morii Yoshihiro
; Version 0.2
;**************************************************
	LIST	P=PIC16F648A
	INCLUDE	"P16F648A.INC"

__CONFIG _EXTCLK_OSC & _WDT_OFF & _CP_OFF & _LVP_OFF & _BODEN_ON & _PWRTE_ON & _MCLRE_OFF

#DEFINE	S87_200	;1/87用
;#DEFINE	S80_200	;1/80用
;#DEFINE	S150_110	;1/150用

;変数の定義
CNT0		EQU	20H		;時間カウンタ下位
CNT1		EQU	21H		;時間カウンタ上位
SPD0		EQU	23H		;速度1桁目
SPD1		EQU	24H		;速度2桁目
SPD2		EQU	25H		;速度3桁目
LEDCNT	EQU	26H		;LED表示用のカウンタ
LEDCNT2	EQU	27H		;LED表示用のカウンタ
LEDCNT3	EQU	28H		;LED表示用のカウンタ
LEDCLM	EQU	29H		;LED表示桁
CFLG		EQU	2AH		;カウント状態フラグ
DTA		EQU	2CH		;PORTA入力値
DTB		EQU	2DH		;PORTB入力値
WCNT1	EQU	30H		;待ちカウンタ
WCNT2	EQU	31H		;待ちカウンタ
WCNT3	EQU	32H		;待ちカウンタ
CNTW_0	EQU	33H		;10倍計算元値下位
CNTW_1	EQU	34H		;10倍計算元値上位
CNTD_0	EQU	35H		;10倍計算結果下位
CNTD_1	EQU	36H		;10倍計算結果上位
CNT10_0	EQU	37H		;時間カウンタ10倍下位
CNT10_1	EQU	38H		;時間カウンタ10倍上位
CNT100_0	EQU	39H		;時間カウンタ100倍下位
CNT100_1	EQU	3AH		;時間カウンタ100倍上位
CNTREF0	EQU	3BH		;カウンタ値の基準値下位
CNTREF1	EQU	3CH		;カウンタ値の基準値上位
DIVW0	EQU	3DH		;割り算の元値、余り下位
DIVW1	EQU	3EH		;割り算の元値、余り上位
DIVWRK	EQU	3FH		;割り算のワーク
W_SV		EQU	40H		;Wレジスタ保管場所
ST_SV	EQU	41H		;STATUS保管場所

VER1		EQU	0EH
VER2		EQU	00H
VER3		EQU	02H

IFDEF	S87_200
;1/87で1km/hの速度で200mm走った時のカウント値62640(mSec)
REFVAL0	EQU	D'176'
REFVAL1	EQU	D'244'
ENDIF

IFDEF	S80_200
;1/80で1km/hの速度で200mm走った時のカウント値57600(mSec)
REFVAL0	EQU	D'00'
REFVAL1	EQU	D'225'
ENDIF

IFDEF	S150_110
;1/150で1km/hの速度で110mm走った時のカウント値59400(mSec)
REFVAL0	EQU	D'8'
REFVAL1	EQU	D'232'
ENDIF

;プログラム先頭
	ORG		0
	GOTO	MAIN
	
;割り込みルーチン
	ORG		4
	GOTO	INTR

;7セグメントLED用のデータを取得する
GET_7SEG 
	ANDLW	0FH			;下位 4bitsのみ有効 
	ADDWF	PCL,F		;Add to PC reg
	RETLW	B'01111110'	;Code 0 
	RETLW	B'00001100'	;Code 1 
	RETLW	B'10110110'	;Code 2
	RETLW	B'10011110'	;Code 3 
	RETLW	B'11001100'	;Code 4 
	RETLW	B'11011010'	;Code 5 
	RETLW	B'11111010'	;Code 6 
	RETLW	B'00001110'	;Code 7 
	RETLW	B'11111110'	;Code 8 
	RETLW	B'11001110'	;Code 9 
	RETLW	B'00111000'	;Code A 
	RETLW	B'10011000'	;Code B 
	RETLW	B'10101000'	;Code C 
	RETLW	B'10110000'	;Code D 
	RETLW	B'11110010'	;Code E 
	RETLW	B'00000000'	;Code F

;タイマ0割り込み処理
INTR
;状態保存
	MOVWF	W_SV			; Wレジスタ退避
	SWAPF	STATUS,W
	MOVWF	ST_SV		; STATUS退避
	BCF		STATUS,RP0

	BCF		INTCON,T0IF	;割り込みフラグクリア                            

;カウンタの再設定
	MOVLW	D'250'
	SUBWF	TMR0

;状態チェック
	BTFSC	CFLG,01H		;カウント開始中でなければスキップ
	GOTO	CNTSPEED		;カウント処理へ
	BTFSC	CFLG,04H		;LED表示OFFならスキップ
	GOTO	DSPLED		;LED表示処理へ
	BTFSC	CFLG,06H		;Ver表示OFFならスキップ
	GOTO	VERDSP		;Ver表示
	GOTO	CLRLED		;LED消灯

CNTSPEED
;カウント処理
	BTFSC	CFLG,07H		;オーバフローフラグOFF
	GOTO	ENDINTR		;カウント終了
	INCF	CNT0,F		;カウンタをインクリメント
	BTFSS	STATUS,Z		;オーバフローしたらスキップ
	GOTO	CNTSPD2		;
	INCF	CNT1,F		;カウンタ2桁目をインクリメント
	BTFSS	STATUS,Z		;オーバフローしたらスキップ
	GOTO	CNTSPD2		;
	BSF		CFLG,07H		;オーバフローフラグON
	MOVLW	0FFH
	MOVWF	CNT0
	MOVWF	CNT1			;最大値を入れる
	GOTO	ENDINTR
CNTSPD2
;LEDの表示
	BTFSC	CFLG,03H		;カウント開始ならスキップ
	GOTO	CNTSPD3
;カウント中LED表示開始
	BSF		CFLG,03H		;カウントLED表示中フラグをセット
	MOVLW	D'1'			;LED表示桁
	MOVWF	LEDCLM
	MOVLW	D'9'			;LED表示を維持するカウント数
	MOVWF	LEDCNT
	MOVLW	D'1'			;LED表示を維持するカウント数
	MOVWF	LEDCNT2
	MOVLW	D'1'			;LED表示値のカウント数
	MOVWF	LEDCNT3
;カウント中LEDの表示カウント処理
CNTSPD3
	DECFSZ	LEDCNT,F		;LEDカウントが0になったらスキップ
	GOTO	ENDINTR		;LED表示の変化はないので割り込み終了
	MOVLW	D'9'			;LED表示を維持するカウント数の設定
	MOVWF	LEDCNT
	RRF		LEDCLM,F		;LED表示桁を右シフト
	BTFSS	STATUS,C		;LED表示桁数分シフトしたらスキップ
	GOTO	CNTSPD4
	MOVLW	D'4'			;LED表示桁
	MOVWF	LEDCLM
CNTSPD4
;表示値のカウント
	DECFSZ	LEDCNT2,F		;LEDカウントが0になったらスキップ
	GOTO	CNTSPD5		;LED表示の変化はない
	MOVLW	D'8'			;LED表示を維持するカウント数の設定
	MOVWF	LEDCNT2
	DECFSZ	LEDCNT3,F		;LEDカウントが0になったらスキップ
	GOTO	CNTSPD5		;LED表示
	MOVLW	D'4'			;LED表示値の設定
	MOVWF	LEDCNT3
CNTSPD5
;LED1桁の表示
	MOVF	LEDCNT3,W		;表示値
	BTFSC	CFLG,02H		;RA3でON
	GOTO	CNTSPD6
	ADDLW	09H
	GOTO	CNTSPD7
CNTSPD6
;RA4でON
	SUBLW	0EH
CNTSPD7
	CALL	DISP
	GOTO	ENDINTR

DSPLED	
;LED表示
	BTFSC	CFLG,05H		;LED表示開始ならスキップ
	GOTO	DSPLED2
;LED表示開始
	BSF		CFLG,05H		;LED表示中フラグをセット
	MOVLW	D'8'			;LED表示桁
	MOVWF	LEDCLM
	MOVLW	D'1'			;LED表示を維持するカウント数
	MOVWF	LEDCNT
;LEDの表示カウント処理
DSPLED2
	DECFSZ	LEDCNT,F		;LEDカウントが0になったらスキップ
	GOTO	ENDINTR		;LED表示の変化はないので割り込み終了
	MOVLW	D'5'			;LED表示を維持するカウント数の設定
	MOVWF	LEDCNT
	RRF		LEDCLM,F		;LED表示桁を右シフト
	BTFSS	STATUS,C		;LED表示桁数分シフトしたらスキップ
	GOTO	DSPLED3
	MOVLW	D'4'			;LED表示桁
	MOVWF	LEDCLM
;LEDを表示する処理
DSPLED3
;どの桁を表示するかチェック
	BTFSC	LEDCLM,02H	;1桁目か
	GOTO	DIGIT1
	BTFSC	LEDCLM,01H	;2桁目か
	GOTO	DIGIT2
DIGIT3
;3桁目の表示
	MOVF	SPD2,W		;3桁目の表示値
	CALL	DISP			;表示
	GOTO	ENDINTR

DIGIT2
;2桁目の表示
	MOVF	SPD1,W		;2桁目の表示値
	CALL	DISP
	GOTO	ENDINTR

DIGIT1
;1桁目の表示
	MOVF	SPD0,W		;1桁目の表示値
	CALL	DISP
	GOTO	ENDINTR
			
VERDSP
;バージョン表示
	MOVLW	VER1
	MOVWF	SPD2
	MOVLW	VER2
	MOVWF	SPD1
	MOVLW	VER3
	MOVWF	SPD0
	CLRF	CFLG
	BSF		CFLG,04H		;速度を表示するフラグON
	GOTO	DSPLED	

CLRLED	
;表示消去
	CLRF	PORTA		;表示を消す
	CLRF	PORTB		;表示を消す
	GOTO	ENDINTR

;割り込み終了
ENDINTR
	SWAPF	ST_SV,W		;Statusを戻す
	MOVWF	STATUS
	SWAPF	W_SV,F		;Wレジスタを戻す
	SWAPF	W_SV,W

	RETFIE
	
;メインプログラム
MAIN
;初期設定
	BSF		STATUS,RP0	;バンク1に切り替え
	MOVLW	01H			;プリスケーラ4
	MOVWF	OPTION_REG	;設定
;I/Oの設定
	;RA0-2	OUTPUT
	;RA3,4	INPUT
	;RB0	INPUT
	;RB1-7	OUTPUT
	MOVLW	B'11111000'
	MOVWF	TRISA
	MOVLW	B'00000001'
	MOVWF	TRISB
	BCF		STATUS,RP0	;バンク0に切り替え
	MOVLW	07H
	MOVWF	CMCON
;初期値設定
	CLRF	CNT0
	CLRF	CNT1
	CLRF	SPD0
	CLRF	SPD1
	CLRF	SPD2
	CLRF	CFLG
	BSF		CFLG,06H
	MOVLW	REFVAL0
	MOVWF	CNTREF0
	MOVLW	REFVAL1
	MOVWF	CNTREF1
;タイマ0の設定
	MOVLW	D'6'			;カウント初期値 256-250
	MOVWF	TMR0			;タイマ0の設定
	BSF		INTCON,T0IE	;タイマ0割り込み許可
	BSF		INTCON,GIE	;全体割り込み許可
;初期設定完了
LOOP
;開始入力チェック
	MOVF	PORTA,W		;PORTAからの入力
	MOVWF	DTA			;値の退避
	BTFSS	DTA,03H		;RA3がOFFならスキップ
	GOTO	RA3ON
	BTFSC	DTA,04H		;RA4がONならスキップ
	GOTO	LOOP			;RA3,4共にOFF
RA4ON					;RA3 OFF && RA4 ON
	BSF		CFLG,02H		;RA4でONを示す
	GOTO	RAON
RA3ON
	BTFSS	DTA,04H		;RA4がOFFならスキップ
	GOTO	LOOP			;RA3,4共にON
	BCF		CFLG,02H		;RA3でONを示す
RAON
	BSF		CFLG,01H		;カウント開始フラグ設定
	BCF		CFLG,04H		;速度を表示するフラグOFF

ENDWAIT
;終了入力チェック
	BTFSC	DTA,03H		;開始時RA3がONならスキップ
	GOTO	SRA4ON
SRA3ON
	BTFSC	PORTA,04H		;RA4がONならスキップ
	GOTO	SRA3ONOVF
	BCF		CFLG,01H		;カウント停止
	GOTO	CALCSPEED
SRA3ONOVF					;オーバーフローのチェック
	BTFSS	CFLG,07H		;オーバーフローフラグが1ならスキップ
	GOTO	SRA3ON
	BCF		CFLG,01H		;カウント停止
	GOTO	LOWSPD
SRA4ON					;開始時RA4がON
	BTFSC	PORTA,03H		;RA3がONならスキップ
	GOTO	SRA4ONOVF
	BCF		CFLG,01H		;カウント停止
	GOTO	CALCSPEED
SRA4ONOVF
	BTFSS	CFLG,07H		;オーバーフローフラグが1ならスキップ
	GOTO	SRA4ON
	BCF		CFLG,01H		;カウント停止
LOWSPD
	CLRF	SPD0			;スピード値のクリア
	CLRF	SPD1
	CLRF	SPD2
	GOTO	CALSPDEND	

CALCSPEED
;速度の計算
	CLRF	SPD0			;スピード値のクリア
	CLRF	SPD1
	CLRF	SPD2
	MOVF	CNTREF0,W		;割り算の引数をセットする
	MOVWF	DIVW0
	MOVF	CNTREF1,W
	MOVWF	DIVW1
	INCF	CNT0,F		;カウンタに1足す
	BTFSC	STATUS,Z
	INCF	CNT1,F
	MOVF	CNT0,W
	MOVWF	CNTW_0
	MOVF	CNT1,W
	MOVWF	CNTW_1
	CALL	MUL10		;カウンタ値を10倍する
	BTFSC	STATUS,C		;桁あふれのチェック
	GOTO	CALSPD3
	MOVF	CNTD_0,W		;計算結果の保存
	MOVWF	CNT10_0
	MOVWF	CNTW_0
	MOVF	CNTD_1,W
	MOVWF	CNT10_1
	MOVWF	CNTW_1
	CALL	MUL10		;カウンタ値10倍を10倍する	
	BTFSC	STATUS,C		;桁あふれのチェック
	GOTO	CALSPD2
	MOVF	CNTD_0,W		;計算結果の保存
	MOVWF	CNT100_0
	MOVWF	CNTW_0
	MOVF	CNTD_1,W
	MOVWF	CNT100_1
	MOVWF	CNTW_1
;最上位の計算
	CALL	DIV10
	MOVWF	SPD2
	SUBLW	D'9'
	BTFSS	STATUS,C		;オーバーフローでない
	GOTO	CALSPDOVF		;オーバーフロー処理へ
	
;2桁目の計算
CALSPD2
	MOVF	CNT10_0,W
	MOVWF	CNTW_0
	MOVF	CNT10_1,W
	MOVWF	CNTW_1
	CALL	DIV10
	MOVWF	SPD1

;最下位桁の計算
CALSPD3
	MOVF	CNT0,W
	MOVWF	CNTW_0
	MOVF	CNT1,W
	MOVWF	CNTW_1
	CALL	DIV10
	MOVWF	SPD0

;四捨五入
	BCF		STATUS,C
	RRF		CNTW_1,F		;カウンタ上位を1/2
	RRF		CNTW_0,F		;カウンタ下位を1/2
	MOVF	CNTW_1,W		;上位を先に計算する
	SUBWF	DIVW1,W
	BTFSS	STATUS,C		;正ならスキップ
	GOTO	CALSPDEND		;四捨
	BTFSS	STATUS,Z		;0ならスキップ
	GOTO	CALSPD4
;下位の計算
	MOVF	CNTW_0,W		;下位を計算する
	SUBWF	DIVW0,W
	BTFSS	STATUS,C		;正ならスキップ
	GOTO	CALSPDEND		;四捨

CALSPD4
;結果に1を加算する
	MOVF	SPD0,W
	SUBLW	D'8'
	BTFSS	STATUS,C		;8以上ならスキップ
	GOTO	CALSPD5
	INCF	SPD0,F
	GOTO	CALSPDEND
CALSPD5
	CLRF	SPD0			;桁上げで最下位を0にする
	MOVF	SPD1,W
	SUBLW	D'8'
	BTFSS	STATUS,C		;8以上ならスキップ
	GOTO	CALSPD6
	INCF	SPD1,F
	GOTO	CALSPDEND
CALSPD6
	CLRF	SPD1			;桁上げで2桁目を0にする
	MOVF	SPD2,W
	SUBLW	D'8'
	BTFSS	STATUS,C		;8以上ならスキップ
	GOTO	CALSPDOVF
	INCF	SPD2,F
	GOTO	CALSPDEND
CALSPDOVF
	MOVLW	D'9'			;オーバーフロー処理
	MOVWF	SPD2
	MOVWF	SPD1
	MOVWF	SPD0

CALSPDEND
	CLRF	CFLG
	BSF		CFLG,04H		;速度を表示するフラグON

MEASUREEND
;計測後の処理
	CALL	WAIT1		;時間待ち
	MOVF	PORTA,W		;PORTAからの入力
	MOVWF	DTA			;値の退避
	BTFSS	DTA,03H		;RA3がOFFならスキップ
	GOTO	MEASUREEND
	BTFSS	DTA,04H		;RA4がOFFならスキップ
	GOTO	MEASUREEND
;2回チェックを行う
	CALL	WAIT1		;時間待ち
	MOVF	PORTA,W		;PORTAからの入力
	MOVWF	DTA			;値の退避
	BTFSS	DTA,03H		;RA3がOFFならスキップ
	GOTO	MEASUREEND
	BTFSS	DTA,04H		;RA4がOFFならスキップ
	GOTO	MEASUREEND
	CLRF	CNT0
	CLRF	CNT1

;ループ
	GOTO	LOOP

;約1m秒時間待ち
WAIT0001
	MOVLW	D'165'
	MOVWF	WCNT1
WLOOP0001
	NOP
	NOP
	NOP
	DECFSZ	WCNT1,F
	GOTO	WLOOP0001
	RETURN

;約0.1秒待ち
WAIT01
	MOVLW	D'99'
	MOVWF	WCNT2
WLOOP01
	CALL	WAIT0001
	DECFSZ	WCNT2,F
	GOTO	WLOOP01
	RETURN
	
;約0.5秒待ち
WAIT1
	MOVLW	D'5'
	MOVWF	WCNT3
WLOOP1
	CALL	WAIT01
	DECFSZ	WCNT3,F
	GOTO	WLOOP1
	RETURN

;カウンタ値を10倍にする
;CNTW_1:CNTW_0*10->CNTD_1:CNTD_0
;桁あふれ:Cフラグ=1
MUL10
;2倍する
	BCF		STATUS,C
	RLF		CNTW_0,F		;カウンタ下位を2倍
	RLF		CNTW_1,F		;カウンタ上位を2倍
	BTFSC	STATUS,C		;桁あふれがあると終わり
	GOTO	MUL10END
;4倍する
	RLF		CNTW_0,W		;カウンタ2倍下位を2倍
	MOVWF	CNTD_0
	RLF		CNTW_1,W		;カウンタ2倍上位を2倍
	BTFSC	STATUS,C		;桁あふれがあると終わり
	GOTO	MUL10END
	MOVWF	CNTD_1
;8倍する
	RLF		CNTD_0,F		;カウンタ4倍下位を2倍
	RLF		CNTD_1,F		;カウンタ4倍上位を2倍
	BTFSC	STATUS,C		;桁あふれがあると終わり
	GOTO	MUL10END
;2倍と8倍を加算する
	MOVF	CNTW_0,W
	ADDWF	CNTD_0,F
	BTFSC	STATUS,C		;桁上げが無い
	INCF	CNTW_1,F
	MOVF	CNTW_1,W
	ADDWF	CNTD_1,F
MUL10END
	RETURN

;割り算を行う
;DIVW1:DIVW0/CNTW_1:CNTW_0->Wreg 余りDIVW1:DIVW0
DIV10
	CLRF	DIVWRK
DIV10_1
	MOVF	CNTW_1,W		;上位を先に計算する
	SUBWF	DIVW1,W
	BTFSS	STATUS,C		;正ならスキップ
	GOTO	DIV10_END
	MOVWF	DIVW1
	BTFSC	STATUS,Z		;上位が0でなければスキップ
	GOTO	DIV10_3
	MOVF	CNTW_0,W		;下位の計算
	SUBWF	DIVW0,W
	BTFSC	STATUS,C		;負ならスキップ
	GOTO	DIV10_2
	DECF	DIVW1,F		;上位から1を引く

DIV10_2
	MOVWF	DIVW0
	INCFSZ	DIVWRK,F		;オーバーフローしたらスキップ
	GOTO	DIV10_1
	DECF	DIVWRK,F
	GOTO	DIV10_END

DIV10_3					;上位が0になった時の計算
	MOVF	CNTW_0,W		;下位の計算
	SUBWF	DIVW0,W
	BTFSC	STATUS,C		;負ならスキップ	
	GOTO	DIV10_4
	MOVF	CNTW_1,W
	ADDWF	DIVW1,F		;引き過ぎたので戻す
	GOTO	DIV10_END

DIV10_4
	MOVWF	DIVW0
	INCFSZ	DIVWRK,F		;オーバーフローしたらスキップ
	GOTO	DIV10_END
	DECF	DIVWRK,F

DIV10_END
	MOVF	DIVWRK,W
	RETURN
	
;LEDの1桁の表示を行う
DISP
	CALL	GET_7SEG		;表示データを取得する
	CLRF	PORTA		;表示を消す
	MOVWF	PORTB		;表示内容を出力する
	MOVF	LEDCLM,W		;表示桁
	MOVWF	PORTA		;表示する
	RETURN
	
	END
