;*************************************************
;
;　　　F103トルクコントロールユニット v11
;　　　　　　with PIC12C509
;
;　　　　GP0 I /受信機信号とりこみ 0で入力なし1で入力あり
;　　　　GP1 O/アンプへの信号を接続(1)／切断(0)
;　　　　GP2　O/ブレーキ動作確認LED 0で点灯 1で消灯
;　　　　GP4 I /センサ入力　0で明 1で暗
;　　　　GP5 O/回転センサー確認LED 0で点灯1で消灯
;
;　　　スポーツチューンモーター、25T/93T
;　　　理論値の100％Table
;　　　ブレーキ90msec
;
;　　　May '00 Masashi Kawanishi
;
;*************************************************
	list p=12c509
	include <p12c509.inc>

;　***　変数レジスタ定義　***

TA	EQU	07H	;　前回タイマー（Ta)
PREDL	EQU	08H	;　直前センサは暗(1)モードか明(0)モードか
TB	EQU	09H	;　現タイマー(Tb)
TTB	EQU	0AH	;　理論Tb
OFTB	EQU	0BH	;　オフセットタイマー
W5M	EQU	0CH	;　wait loop用変数
W1M	EQU	0DH	;　同上
W160U	EQU	0EH	;　同上
MMK	EQU	0FH	;　タイマ上限

RESET
	GOTO	START
	ORG	04H

;*********　ここから　*************

START	MOVLW	09FH
	OPTION

	MOVLW	019H	;0001 1001 (1で入力0で出力)
	TRIS	GPIO

	BSF	GPIO,1	;1bitをHiにする(アンプ接続）
	BSF	GPIO,5	;5bitをHiにする（LED消灯）
	BSF	GPIO,2	;2bitをHiにする（LED消灯）

	MOVLW	025H	;オフセットタイマー37サイクル
	MOVWF	OFTB	; ＝2.59ms
	
	MOVLW	0FDH
	MOVWF	MMK

	CLRF	TA
	CLRF	TB
	CLRF	PREDL

;　***　メイン　***

MAIN
	BTFSC	PREDL,0	;直前センサが0(明）だったら次をスキップ
	GOTO	DARK	;直前センサが1(暗）だったらDARKへ

;***前回ループではマーカー内であった***

	BTFSS	GPIO,4	;現在センサが1(暗）だったら次をスキップ
	GOTO	CNT5	;現在センサが0(明）だったらカウントへ
	NOP
	NOP
	BTFSS	GPIO,4	;同じ確認をもう二回(ノイズ対策）
	GOTO	CNT9
	NOP
	NOP
	BTFSS	GPIO,4
	GOTO	CNT13

	;**回転マーカーが切れた**

	BSF	GPIO,5	;LED消灯
	MOVLW	01H
	MOVWF	PREDL
	GOTO	CNT17

;***前回ループはマーカー外であった***

DARK	BTFSC	GPIO,4	;現在センサが0(明）だったら次をスキップ
	GOTO	CNT6	;現在センサが1(暗）だったらカウントへ
	NOP
	NOP
	BTFSC	GPIO,4	;同じ確認をもう二回(ノイズ対策）
	GOTO	CNT10
	NOP
	NOP
	BTFSC	GPIO,4
	GOTO	CNT14	

	;**回転マーカーを検出**

	BCF	GPIO,5	;LED点灯 14
	MOVLW	00H
	MOVWF	PREDL	;16

	;***TAの値からTTBを求め、"TTB"に代入

	INCFSZ	TA,W	;TA=FFHだったら次をスキップ
	GOTO	REF1
	MOVLW	0CFH
	GOTO	REFEND21	;21

REF1	MOVLW	08EH	;20
	SUBWF	TA,W		;TA-8E
	BTFSS	STATUS,C	;STATUS,Cはボローが発生すると0
	GOTO	REF7	;24	;Cが0のとき＝TAが8D以下はREF7
				;1の時は8E以上なのでGOTO文をskip

REF2	MOVLW	0DEH	;24	;TA <= 0FEH
	SUBWF	TA,W		;TA-0DEH
	BTFSS	STATUS,C	;ボローが発生しなかったら(TA=DEも)スキップ
	GOTO	REF3
	MOVLW	02AH
	SUBWF	TA,W
	GOTO	REFEND31	;31

REF3	MOVLW	0C0H	;29		;TA= DD〜C0
	SUBWF	TA,W
	BTFSS	STATUS,C
	GOTO	REF4
	MOVLW	01FH
	SUBWF	TA,W
	GOTO	REFEND36	;36

REF4	MOVLW	0AFH	;34	;BF〜AF
	SUBWF	TA,W
	BTFSS	STATUS,C
	GOTO	REF5
	MOVLW	017H
	SUBWF	TA,W
	GOTO	REFEND41	;41

REF5	MOVLW	09BH	;39	;AE〜9B
	SUBWF	TA,W
	BTFSS	STATUS,C
	GOTO	REF6
	MOVLW	012H
	SUBWF	TA,W
	GOTO	REFEND46	;46

REF6				;9A〜8E 8E以上は判定済み
	MOVLW	00EH	;44
	SUBWF	TA,W
	GOTO	REFEND47	;47

REF7	MOVLW	073H	;25	;8D〜73
	SUBWF	TA,W
	BTFSS	STATUS,C
	GOTO	REF8	;29
	MOVLW	00AH
	SUBWF	TA,W
	GOTO	REFEND32	;32

REF8	MOVLW	05EH	;30
	SUBWF	TA,W
	BTFSS	STATUS,C
	GOTO	REF9
	MOVLW	006H
	SUBWF	TA,W
	GOTO	REFEND37	;37

REF9	MOVLW	04AH	;35
	SUBWF	TA,W
	BTFSS	STATUS,C
	GOTO	REF10
	MOVLW	003H
	SUBWF	TA,W
	GOTO	REFEND42	;42

REF10	MOVLW	03BH	;40
	SUBWF	TA,W
	BTFSS	STATUS,C
	GOTO	REF11
	MOVLW	002H
	SUBWF	TA,W
	GOTO	REFEND47	;47

REF11	MOVLW	001H	;45
	SUBWF	TA,W
	BTFSC	STATUS,C	;
	GOTO	REFEND49	;49
	CLRW
	GOTO	REFEND49	;(51)


REFEND21	NOP
	NOP
	NOP
	NOP
	NOP	;
	NOP
	NOP
	NOP
	NOP
	NOP	;
REFEND31	NOP
REFEND32	NOP
	NOP
	NOP
	NOP	;
REFEND36	NOP
REFEND37	NOP
	NOP
	NOP
	NOP
REFEND41	NOP
REFEND42	NOP
	NOP
	NOP
	NOP	;
REFEND46	NOP
REFEND47	NOP
REFEND48	NOP
REFEND49
	

	SUBWF	TB,W	;TB-TTB
	BTFSC	STATUS,C	;TB < TTB だったら次をスキップ

	GOTO	NORM	;通常の加速

;****ホイールスピン発生***

	BCF	GPIO,2	;LED点灯

	;**30ms信号をカット**
WTNS	BTFSC	GPIO,0
	GOTO	WTNS	;受信機からの信号がなくなるまで待つ

	BCF	GPIO,1	;6番ピンをLoにする

WAIT30M	;***90msec待ちループ***

	MOVLW	5AH
	MOVWF	W5M
W5
	MOVLW	0F9H
	MOVWF	W1M
W1
	NOP
	DECFSZ	W1M,	F
	GOTO	W1

	DECFSZ	W5M,	F
	GOTO	W5

;************ここまで************

WTNS2	BTFSC	GPIO,0
	GOTO	WTNS2	;受信機からの信号がなくなるまで待つ

	BSF	GPIO,1	;6番ピンをHiにする

	BSF	GPIO,2	;LED消灯

;	;**変数リセット**
	CLRF	TA
	CLRF	TB
	CLRF	PREDL
	MOVLW	025H	;オフセットタイマー2.5ms
	MOVWF	OFTB

	GOTO	MAIN

;****正常な加速****

NORM	MOVF	TB,W	;54
	MOVWF	TA	;Ta ← Tb
	CLRF	TB
	MOVLW	025H	;オフセットタイマー2.5ms	
	MOVWF	OFTB	;

	GOTO	CNT60

;経路による時差を解消、タイマをカウントアップし、メインに戻る(GOTOで）

CNT5	NOP
CNT6	NOP
	NOP
	NOP
CNT9	NOP
CNT10	NOP
	NOP
	NOP
CNT13	NOP
CNT14	NOP
	NOP
CNT16	NOP
CNT17	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
CNT29	NOP
	NOP
	NOP
CNT32	NOP
CNT33		;ここ終了時で33us
	MOVLW	08H	;7回ループ
	MOVWF	W160U	;35us
CNTMN	DECFSZ	W160U,F	;+3*7+2=
	GOTO	CNTMN	;58us
	NOP
	NOP

CNT60	DECFSZ	OFTB,F	;OFTBを-1して00だったら次をスキップ
	GOTO	OFCNT

	;***TBカウント***
	INCF	OFTB,F	;OFTBを戻す

	NOP
	NOP
	NOP
	NOP	;67

	INCFSZ	TB,F	;TBを+1,0なら次をスキップ

	GOTO	MAIN	;70usでリターン

	MOVLW	0FFH	;TB=0FFHなので時間は関係なくリターン
	MOVWF	TB
	GOTO	MAIN	;73us

OFCNT	;（OFTBをすすめたので）時間を合わせてgotoMAIN
	NOP	;64
	NOP
	NOP
	NOP
	NOP
	GOTO	MAIN	;70usでリターン


	END