; Формирователь задержки заднего фронта и длительности в зависимости от периода входных импульсов.
; При Т от ∞ до 30мс и меньше 4.7мс задержка 0, между в зависимости от Т.
; 1, 2-	RA2 RA3 входы противофаза скважность 24 активность лог0
; 6 -	RB0 выход синхронный с RB6
; 11, 12 RB5 RB6 выходы попарно синхронно с входами длит 1/2 Т с задержкой в диап 30мс - 4.7мс
 list	p=16f627a
#include	p16f627a.inc

#define Bank0	bcf	STATUS,RP0
#define Bank1	bsf	STATUS,RP0

	__CONFIG _CP_OFF & _CPD_OFF & _LVP_OFF & _BODEN_ON & _MCLRE_ON & _PWRTE_ON & _WDT_OFF & _INTRC_OSC_NOCLKOUT

; ==========================адреса=======================
	CBLOCK 0x20
;  AL     AM      BL   BM        CL     CM      DL      DM
 hiAA.H,hiAA.L,loDPZ.H,loDPZ.L,loAA.H,loAA.L,hiDPZ.H,hiDPZ.L,xDPZ.H,xDPZ.L
 AB.L,AB.M,AB.H,AB.U,CD.L,CD.M,CD.H,CD.U,CD.UL
 RES.L,RES.M,RES.H,RES.U,REG.AL,REG.AM,REG.AH				; REG.AH - лишний?
 Q.REG.L,Q.REG.M,Q.REG.H,REZ.L,REZ.M,REZ.H,REZ.U,REZ.UL
; loT.H:loT.L период в точке графика, соотв меньшим оборотам hi-lo относительно оборотов, на самом деле период больше, чем hiT
; hiT.H:hiT.L период в точке графика, соотв большим оборотам hi-lo относительно оборотов, на самом деле период меньше, чем loT
 loT.H,loT.L,hiT.H,hiT.L
 cnt_loop		; регистр проходов антидребезга
 T.H,T.L		; Время полуоборота в тиках TMR OSC/4
 ton.H,ton.L		; время накопления в тиках TMR OSC/4
 toff.H,toff.L		; время ожидания, выдержка, в тиках TMR OSC/4
 Dact			; RA2-3 D1 || D2 актив лог0, активный канал, между задними фронтами
 Stsflg			; флаги состояния: 3:ovflwTMRT 2:_AA.0, 1:AA.0, 0:ICflg
 Dlvl			; RA2-3 D1 || D2 актив лог0, моментальный реальный уровень датчиков, для обнаружения инверсии
 pTMRoff,TMRoff,pTMRon,TMRon,_pTMRoff,_TMRoff,_pTMRon,_TMRon	; время вкл бобин вычисленная для загруза в TMRA, _подчерк актив
 Rtmp2,Rtmp1,Rtmp0	; вспом регистры	57 штук
	ENDC

	CBLOCK	0x70
 w_sav,stat_sav,pclath_sav
 AA.H,AA.L		; AA.H:AA.L вычисляемый УОЗ в тиках TMR OSC/4, отступ до зад фронта RearFront
	ENDC

ovflwTMRT equ	0x3	; флаг переполнения TMRT, оч малые обороты, вруб ICon, не запускать TMRA, выкл ICoff по RearFront
ICflg	equ	0	; флаг вкл/выкл бобин Stsflg 0бит, сигнализирует о вкл состоянии бобин
_AA.0	equ	0x2	; флаг состояния AA=0 в Stsflg в предыдущем периоде
AA.0	equ	0x1	; флаг состояния AA=0 в Stsflg в текущем периоде
IC1	equ	0x5	; бобина 1Ц RB5
IC2	equ	0x6	; бобина 2Ц RB6
TAHO	equ	0	; тахометр RB0
TMRA	EQU	0x1	; TMR0 отсчёт toff ton AA (УОЗ)
TMRT.H	EQU	0xF	; TMR1 измерение периода
TMRT.L	EQU	0xE
D1	equ	0x2	; RA2 sense in, active LOW ↓↑
D2	equ	0x3	; RA3 sense in, active LOW ↓↑
m1T.H	equ	0x75	; ======КОНСТАНТЫ================================================================
m1T.L	equ	0x30	; период 1/2 оборота в тиках TMR OSC/4 LimitRPMin ниже УОЗ=0
m2T.H	equ	0x3A
m2T.L	equ	0x98	; период в точке 2000 об/мин
m3T.H	equ	0x27
m3T.L	equ	0x10	;		КОНТРОЛЬНЫЕ ПЕРЕЛОМНЫЕ ТОЧКИ кривой УОЗ
m4T.H	equ	0x1D	;		НЕ ПОДЛЕЖАТ ИЗМЕНЕНИЮ		НЕ ПОДЛЕЖАТ ИЗМЕНЕНИЮ
m4T.L	equ	0x4C
m5T.H	equ	0x17	; период в точке 5000 об/мин
m5T.L	equ	0x70
m6T.H	equ	0x13
m6T.L	equ	0x88
m7T.H	equ	0x12	; LimitRPMax огр макс обороты, выше УОЗ=0, в тиках TMR OSC/4
m7T.L	equ	0x99	; 6300 RPM
m1AA.H	equ	0	; ==========КОНСТАНТЫ============================================================
m1AA.L	equ	0	; 0° УОЗ в точке 1000 об/мин
m2AA.H	equ	0x4
m2AA.L	equ	0xE2	; 15° УОЗ в точке 2000 об/мин
m3AA.H	equ	0x4
m3AA.L	equ	0x57	; 20° УОЗ в точке 3000 об/мин
m4AA.H	equ	0x3
m4AA.L	equ	0xBE	; 23° УОЗ в точке 4000 об/мин		ПОДЛЕЖАТ ИЗМЕНЕНИЮ, УОЗ В КОНТРОЛЬНЫХ ТОЧКАХ
m5AA.H	equ	0x3
m5AA.L	equ	0x41	; 25° УОЗ в точке 5000 об/мин		ПЕРЕЛОМА КРИВОЙ УОЗ
m6AA.H	equ	0x2
m6AA.L	equ	0xD2	; 26° УОЗ в точке 6000 об/мин
m7AA.H	equ	0x2
m7AA.L	equ	0xAF	; 26° УОЗ в точке 6300 об/мин
loop_drbzg equ	0x5	; число проходов антидребезга
fTMRT.H	equ	0x7f	; для первого полуоборота TMRT=FFFF, эмуляция периода ≈65мс
;fTMRT.L	equ	0xff	; f - значит FIRST! stage
delay.cycle equ	0xf	; 7 = 9000 циклов задержка для сдвига вкл катушек сразу после зад фронта на пуск оборотах
;;fpTMRoff equ	0x6	; toff=T-ton=C000		чушь, первые полуобороты TMRA не работает
;;fTMRoff	equ	0x3F	; для первого полуоборота
;;fpTMRon	equ	0x5	; ton=T/4=3FFF
;;fTMRon	equ	0	; для первого полуоборота ГТО

	org	0
	goto	Init		; Start
	org	0x4
	movwf	w_sav		; save context
	swapf	STATUS,w
	movwf	stat_sav
	movf	PCLATH,w	; only required if using more than first page
	movwf	pclath_sav
	clrf	PCLATH
;		if ICflag=0 call ICon ; if Stsflg:_AA.0=0 start TMRA (ton) ; retfie
;		else call ICoff ; retfie
;	else TMRT stop TMRT ; set ovflwTMRT ; set AA.0?? ; 0xFF → TMRT ; retfie  факт переполнения на малых оборотах, значение для след цикла
	btfsc	PIR1,TMR1IF	; Bank 0 PIR1 = EEIF CMIF RCIF TXIF — CCP1IF TMR2IF TMR1IF
	goto	int.TMRT
	btfsc	Stsflg,ICflg	; int.TMRA
	goto	gn.ICoff
	call	ICon		; если бобины выключены
	btfsc	Stsflg,_AA.0
	goto	stp.TMRA	; AA=0, не запускаем TMRA
	movfw	_TMRon		; AA≠0, запускаем TMRA (_ton)
	movwf	TMRA
	bcf	INTCON,T0IF
	bcf	INTCON,T0IE
	movfw	_pTMRon
	Bank1			; Bank 1,3 OPTION:0:[PS2:0] <- fpTMRon
	movwf	OPTION_REG	; /RBPU INTEDG T0CS T0SE PSA PS2 PS1 PS
	Bank0
	bsf	INTCON,T0IE	; старт TMRA (_ton)
	goto	exit.int
gn.ICoff: call	ICoff		; если бобины включены, выкл - искра
stp.TMRA: bcf	INTCON,T0IE	; Bank all INTCON 5..2 GIE PEIE T0IE INTE RBIE T0IF INTF RBIF
	goto	exit.int
; Bank 0 TMRT.H:TMRT.L
int.TMRT: clrf	T1CON		; Bank 0 T1CON = U U T1CKPS1 T1CKPS0 T1OSCEN T1SYNC TMR1CS TMR1ON - все 0
	clrf	PIR1		; Bank 0 PIR1 = EEIF CMIF RCIF TXIF — CCP1IF TMR2IF TMR1IF
	movlw	0xff
	movwf	TMRT.L
	movwf	TMRT.H		; факт переполнения на малых оборотах, значение для след цикла
;	bcf	STATUS,C
	rrf	TMRT.H,f	; 7FFF
	bsf	Stsflg,ovflwTMRT
;	bsf	Stsflg,AA.0
exit.int: movf	pclath_sav,w	; restore context
	movwf	PCLATH
	swapf	stat_sav,w
	movwf	STATUS
	swapf	w_sav,f
	swapf	w_sav,w
	retfie

;							pin	func
;							1, 2 -	RA2 RA3 sense in
Init:	movlw	b'111'		; ComparatorS OF	7, 8 -	RB1 RB2 +/- uoz in
	movwf	CMCON		;			9 -	RB3 switch in
;	clrf	PORTA		;			4 -	MCLR
	clrf	PORTB		;			6 -	RB0 taho out
	Bank1			;			11, 12	RB5 RB6 spark out
	movlw	b'11111111'
	movwf	TRISA		; 0xFF ; Все ноги в Z как входы, ВКЛЮЧИТЬ ПОДТЯЖКИ!!!
	movlw	b'10011110'	; ноги в Z как входы, кроме 0 5 6
	movwf	TRISB
	clrf	OPTION_REG	;  /RBPU INTEDG T0CS T0SE PSA PS2 PS1 PS0
	clrf	VRCON
	Bank0
;	PCON	OSCF — POR BOR
; конфиг TMRT - TMR1
; Bank 0 TMRT.H:TMRT.L
; Bank 0 T1CON = U U T1CKPS1 T1CKPS0 T1OSCEN T1SYNC TMR1CS TMR1ON - все 0, кроме TMR1ON
	CLRF	T1CON		; Stop Timer1, Internal Clock Source, ; T1 oscillator disabled, prescaler = 1:1
;	clrf	TMRT.H
;	clrf	TMRT.L
	clrf	INTCON		; Bank all INTCON = GIE PEIE T0IE INTE RBIE T0IF INTF RBIF
	Bank1
	clrf	PIE1		; Bank 1   PIE= EEIE CMIE RCIE TXIE — CCP1IE TMR2IE TMR1IE
	bsf	PIE1,TMR1IE
	Bank0
	CLRF	PIR1		; Bank 0 PIR1 = EEIF CMIF RCIF TXIF — CCP1IF TMR2IF TMR1IF
; конфиг TMRA - TMR0
; Bank 1,3 OPTION:0:[PS2:0] <- fpTMRoff
; Bank 0,2 TMR0 TMRA
;	clrf	TMRA
;	clrf	INTCON		; above in init TMRT
;	Bank1
;	clrf	OPTION_REG	; /RBPU INTEDG T0CS T0SE PSA PS2 PS1 PS0
;	Bank0
;	movlw	b'01000000'
	bsf	INTCON,PEIE	; Bank all INTCON = GIE PEIE T0IE INTE RBIE T0IF INTF RBIF

; Зарядка первозначений для начала
;	movlw	fTMRT.L		; для первого полуоборота TMRT=FFFF, эмуляция периода ≈65мс
	clrf	TMRT.L		; рассчитано вручную
	movlw	fTMRT.H
	movwf	TMRT.H
;	movlw	fpTMRoff
;	movwf	pTMRoff
;	movlw	fTMRoff
;	movwf	TMRoff
;	movlw	fpTMRon
;	movwf	pTMRon
;	movlw	fTMRon
;	movwf	TMRon
	clrf	Stsflg
	bsf	Stsflg,AA.0	; для 1st AA=0
	bsf	Stsflg,ovflwTMRT

; Первоначальный скан обоих датчиков, (↓ call ICon && goto FwdFront) || (↑ nop)
ScanDfls: movlw	loop_drbzg	; установка числа проходов с повторяющимся рез-том, для стабильности
	movwf	cnt_loop
ScanDtru: movf	PORTA,w		; быстрый поиск активного входа
	andlw	0xC		; выделение RA2 RA3 по маске, актив лог0 ((1 << RA3) || (1 << RA2))
	movwf	Dlvl		; W=Dlvl=PORTA
	btfss	Dlvl,D1		; инверсная логика, активность лог0
	goto	ActDs
	btfsc	Dlvl,D2		; инверсная логика, активность лог0
	goto	ScanDtru		; ничего, все входы неактивны
ActDs:	movf	PORTA,w		; сравнение с предыдущим
	andlw	0xC		; выделение RA2 RA3 по маске, актив лог0 ((1 << RA3) || (1 << RA2))
	subwf	Dlvl,w		; сравнение с предыдущим циклом антидребезга
	btfss	STATUS,Z
	goto	ScanDfls	; не совпадает, сброс счётчика проходов, в начало
	decfsz	cnt_loop,f	; один из Д сработал, совпад с пред, декремент счётчика проходов
	goto	ActDs		; на след проход scanDs1

	movf	Dlvl,w		; Dlvl=PORTA loop_drbzg раз повторяющийся, D1 || D2 (2 3 bit) активен лог0
	movwf	Dact		; W=Dlvl=Dact, дальше их роли расходятся
	call	ICon		; первый имп, врубить бобину сразу
;;	goto	FwdFront

; ПП отр фронта ↓, Ds 1->0, Dlvl D1 || D2 -> 0 , надо уложиться в ширину шторки
; Чтение таймера периода TMRT.H:TMRT.L в регистр периода T.H:T.L и рестарт
; The prescaler counter is cleared on writes to the TMR1.H or TMR1.L registers
; mov [p]TMR[on|off] -> _[p]TMR[on|off]
FwdFront: bcf	INTCON,GIE	; Запрет прерываний??
	clrf	T1CON		; Bank 0 T1CON = U U T1CKPS1 T1CKPS0 T1OSCEN T1SYNC TMR1CS TMR1ON - все 0, кроме TMR1ON
	clrf	PIR1		; Bank 0 PIR1 = EEIF CMIF RCIF TXIF — CCP1IF TMR2IF TMR1IF
	movf	TMRT.H,W
	movwf	T.H
	movf	TMRT.L,W
	movwf	T.L
	clrf	TMRT.H
	clrf	TMRT.L
	bsf	T1CON,TMR1ON
	bsf	INTCON,GIE	; Bank all INTCON = GIE PEIE T0IE INTE RBIE T0IF INTF RBIF
	movfw	pTMRoff		; mov [p]TMR[on|off] -> _[p]TMR[on|off]
	movwf	_pTMRoff	; перенос свежих данных в активные для следующ периода
	movfw	TMRoff		; в таймер грузить предчерк
	movwf	_TMRoff
	movfw	pTMRon
	movwf	_pTMRon
	movfw	TMRon
	movwf	_TMRon
	btfsc	Stsflg,AA.0	; Копир предыд для работы
	bsf	Stsflg,_AA.0
	btfss	Stsflg,AA.0
	bcf	Stsflg,_AA.0
;	goto	ScanD

; Скан D1 || D2 = f(Dact) на инверсию поочерёдно по пред уровню Dlvl
; (↓ goto FwdFront) || (↑ goto RearFront)
; Dact D1 || D2 актив лог0 == Dlvl пред цикл
; опр активный D1 || D2 in Dact
ScanD:	btfsc	Dact,D1		; activD1? актив лог0
	goto	ActD2		; нет, D2
	movfw	Dlvl		; activD1:
	andlw	0x4
	movwf	Rtmp0
LoopD1:	movlw	loop_drbzg	; установка числа проходов с повторяющимся рез-том, для стабильности
	movwf	cnt_loop
ScanD1:	movf	PORTA,w
	andlw	0x4		; сравнение D1 RA2 по маске с предыд состоянием, инверсия входа?
	xorwf	Rtmp0,w		; сравнение с предыд состоянием
	btfsc	STATUS,Z	; если Z=1, уровень не изменился
	goto	LoopD1		; уровень не изменился
	decfsz	cnt_loop,f	; уровень изменился, декремент счётчика проходов
	goto	ScanD1		; на след проход
; RA2 изменил уровень. Как? Спад или подъём? Факт смены уровня подразумавает инверсию D1 in Dlvl
; Просто инвертируем его, если был 0 -> 1 && goto RearFront, 1 -> 0 && goto FwdFront
	movlw	0x4		; вес D1
	xorwf	Dlvl,f		; инверсия D1 по факту соответствия PORTA
	btfss	Dlvl,D1
	goto	FwdFront
	goto	RearFront

ActD2:	movfw	Dlvl		; activD2:
	andlw	0x8
	movwf	Rtmp0
LoopD2:	movlw	loop_drbzg	; установка числа проходов с повторяющимся рез-том, для стабильности
	movwf	cnt_loop
ScanD2: movf	PORTA,w
	andlw	0x8		; выделение D2 RA3 по маске
	xorwf	Rtmp0,w		; сравнение с предыд состоянием
	btfsc	STATUS,Z	; если Z=1, уровень не изменился
	goto	LoopD2
	decfsz	cnt_loop,f	; уровень изменился, декремент счётчика проходов
	goto	ScanD2		; на след проход
; RA3 изменил уровень. Как? Спад или подъём? Факт смены уровня подразумавает инверсию D2 in Dlvl
; Просто инвертируем его, если был 0 -> 1 && goto RearFront, 1 -> 0 && goto FwdFront
	movlw	0x8		; вес D2
	xorwf	Dlvl,f		; инверсия D2 по факту соответствия PORTA
	btfss	Dlvl,D2
	goto	FwdFront
;	goto	RearFront

; ПП положит фронта ↑, Ds 0->1, Dlvl D1 || D2 -> 1
; загруз таймера выдержки pTMRA:TMRA из _pTMRoff:_TMRoff и его старт
; The prescaler counter is cleared on writes to the TMRA registers
; stop TMRA ; if ICflag=1 call ICoff		; Bank 1,3 OPTION:0:[PS2:0]
; start TMRA (toff) ; swap D1-D2 Dflag		; Bank 0,2 TMR0 TMRA
; _pTMRoff _TMRoff swap activ D1-D2		; Bank all INTCON 5 2 GIE PEIE T0IE INTE RBIE T0IF INTF RBIF
RearFront: bcf	INTCON,GIE	; stop TMRA Запрет прерываний?
	btfsc	Stsflg,ICflg	; если бобины включены
	call	ICoff		; выкл = искра
	movlw	0xC		; маска весов D1 D2 in Dact
	xorwf	Dact,f		; swap D1 <-> D2, смена активного цилиндра
	btfss	Stsflg,ovflwTMRT
	goto	strtTMRA
	bcf	Stsflg,ovflwTMRT
	call	delay.ms
	call	ICon
	goto	exit.RrFr
strtTMRA: movfw	_TMRoff
	movwf	TMRA
	bcf	INTCON,T0IF
	bcf	INTCON,T0IE
	movfw	_pTMRoff
	Bank1
	movwf	OPTION_REG	; /RBPU INTEDG T0CS T0SE PSA PS2 PS1 PS
	Bank0
	bsf	INTCON,T0IE	; старт TMRA _toff
exit.RrFr bsf	INTCON,GIE
	bcf	STATUS,RP1	; раз за цикл на всяк случай

; =======================================================================================
; compAA Вычисление УОЗ и сопутствующих ton toff
; 1) comp ton.H:ton.L = T.H:T.L/4 (>>2), ton.H:ton.L сохран
; 2) let  ton.H = ton.H:ton.L/FF (>>8) 256 - макс Коэф деления предделителя, ton.H:ton.L сохран
; 3) pTMRon = wghtMSB вес бита, след за старш ед битом в ton.H -> OPTION:0:[PS2:0]
; 4) TMRon = FF-ton.H:ton.L/(вес позиции+1 MSB) где wghtMSB = закодированный Коэф в OPTION:0:[PS2:0], ton.H:ton.L сохран
;
; 1) comp ton.H:ton.L = T.H:T.L/4 (>>2), ton.H:ton.L сохран
compAA:	BCF	STATUS,C
	RRF	T.H,W
	movwf	ton.H
	RRF	T.L,W
	movwf	ton.L		; ton = T/2
;	BCF	STATUS,C
;	RRF	ton.H,F
;	RRF	ton.L,F		; ton = T/4
; 2) let  ton.H = ton.H:ton.L/FF (>>8) 256 - макс Коэф деления предделителя
; 3) pTMR0n = wghtMSB вес бита, след за старш ед битом в ton.H -> OPTION:0:[PS2:0]
; ®одэрик
	movf	ton.H,W		; ton.H = ton.H:ton.L/FF (>>8), ton.H:ton.L сохран
	movwf	Rtmp2
ip1:	movlw	0x7		; заготовка 0111 для OPTION:0:PS[2:0]
	movwf	Rtmp1
ip1.it1: btfsc	Rtmp2,7		; проверка старшего бита
	goto	ip1.rez	
	rlf	Rtmp2,F		; выдвижение следующего бита ???
	decfsz	Rtmp1,F
	goto	ip1.it1
ip1.rez: movf	Rtmp1,W
	movwf	pTMRon		; pTMRon = OPTION:00000:PS[2:0] готовый предделитель
; TMRon = FF-ton.H:ton.L/(вес MSB + 1), ton.H:ton.L сохран
; деление на 256 128.....4 2
	movf	ton.H,W
	movwf	Rtmp2
	movf	ton.L,W		; ton.H:ton.L сохран
	movwf	Rtmp1
	incf	pTMRon,W	; кол-во сдвигов соответственно значению делителя OPTION:0:[PS2:0] + 1
div.ton: BCF	STATUS,C
	RRF	Rtmp2,F
	RRF	Rtmp1,F
	addlw	0xff		; decrement W
	btfss	STATUS,Z
	goto	div.ton
	movf	Rtmp1,W		; большое вход значение - много сдвигов, малое - мало
	sublw	0xff		; TMRA считает от 0 -> FF
	movwf	TMRon		; TMRon готовый

; 1) if T.H:T.L < 6.3T || T.H:T.L > 1T ; then AA=0 ; else AA=f(T.H:T.L)
; 2) find xDPZ - диапазон оборотов до LmRPMin (1000), 1000-2000, 2к-3к,
;            3к-4к, 4к-5к, 5к-6к, 6к-lmRPMax (6.3k), выше lmRPMax
; 3) let loAA hiAA loT hiT, for actual xDPZ
; 4) loDPZ=loT-T					sub 2b - 2b
; 5) xDPZ=						mul 2b * 2b
; 5) hiDPZ=xDPZ-loDPZ					add 4b + 4b
; 6) AA=(hiAA*loDPZ+loAA*hiDPZ)/xDPZ			div 5b : 2b

; 1) if T.H:T.L < 6.3T ; then AA=0
				; if( X <= K )
	movfw	T.H		;  movfw XH
	sublw	m7T.H		;  sublw KH ;not available on 12 bit core
	skpc			;  skpc
	goto	Tmore7		;  goto endif
	skpz			;  skpz
	goto	Tmore1		;  goto then
	movfw	T.L		;  movfw XL
	sublw	m7T.L		;  sublw KL
	skpc			;  skpc
	goto	Tmore7		;  goto endif
	goto	Tmore1		; then:
				; endif:
; 2) find xDPZ - диапазон оборотов до LmRPMin (1000), 1000-2000, 2к-3к,
;            3к-4к, 4к-5к, 5к-6к, 6к-lmRPMax (6.3k), выше lmRPMax
; 3) let hiAA loAA hiT loT, for actual xDPZ
Tmore7:	movfw	T.H
	sublw	m6T.H
	skpc
	goto	Tmore6
	skpz
	goto	let6DPZ
	movfw	T.L
	sublw	m6T.L
	skpc
	goto	Tmore6
let6DPZ: movlw	m7T.H
	movwf	hiT.H
	movlw	m7T.L
	movwf	hiT.L
	movlw	m6T.H
	movwf	loT.H
	movlw	m6T.L
	movwf	loT.L
	movlw	m7AA.H
	movwf	hiAA.H
	movlw	m7AA.L
	movwf	hiAA.L
	movlw	m6AA.H
	movwf	loAA.H
	movlw	m6AA.L
	movwf	loAA.L
	goto	comp.loDPZ
Tmore6:	movfw	T.H
	sublw	m5T.H
	skpc
	goto	Tmore5
	skpz
	goto	let5DPZ
	movfw	T.L
	sublw	m5T.L
	skpc
	goto	Tmore5
let5DPZ: movlw	m6T.H
	movwf	hiT.H
	movlw	m6T.L
	movwf	hiT.L
	movlw	m5T.H
	movwf	loT.H
	movlw	m5T.L
	movwf	loT.L
	movlw	m6AA.H
	movwf	hiAA.H
	movlw	m6AA.L
	movwf	hiAA.L
	movlw	m5AA.H
	movwf	loAA.H
	movlw	m5AA.L
	movwf	loAA.L
	goto	comp.loDPZ
Tmore5:	movfw	T.H
	sublw	m4T.H
	skpc
	goto	Tmore4
	skpz
	goto	let4DPZ
	movfw	T.L
	sublw	m4T.L
	skpc
	goto	Tmore4
let4DPZ: movlw	m5T.H
	movwf	hiT.H
	movlw	m5T.L
	movwf	hiT.L
	movlw	m4T.H
	movwf	loT.H
	movlw	m4T.L
	movwf	loT.L
	movlw	m5AA.H
	movwf	hiAA.H
	movlw	m5AA.L
	movwf	hiAA.L
	movlw	m4AA.H
	movwf	loAA.H
	movlw	m4AA.L
	movwf	loAA.L
	goto	comp.loDPZ
Tmore4:	movfw	T.H
	sublw	m3T.H
	skpc
	goto	Tmore3
	skpz
	goto	let3DPZ
	movfw	T.L
	sublw	m3T.L
	skpc
	goto	Tmore3
let3DPZ: movlw	m4T.H
	movwf	hiT.H
	movlw	m4T.L
	movwf	hiT.L
	movlw	m3T.H
	movwf	loT.H
	movlw	m3T.L
	movwf	loT.L
	movlw	m4AA.H
	movwf	hiAA.H
	movlw	m4AA.L
	movwf	hiAA.L
	movlw	m3AA.H
	movwf	loAA.H
	movlw	m3AA.L
	movwf	loAA.L
	goto	comp.loDPZ
Tmore3:	movfw	T.H
	sublw	m2T.H
	skpc
	goto	Tmore2
	skpz
	goto	let2DPZ
	movfw	T.L
	sublw	m2T.L
	skpc
	goto	Tmore2
let2DPZ: movlw	m3T.H
	movwf	hiT.H
	movlw	m3T.L
	movwf	hiT.L
	movlw	m2T.H
	movwf	loT.H
	movlw	m2T.L
	movwf	loT.L
	movlw	m3AA.H
	movwf	hiAA.H
	movlw	m3AA.L
	movwf	hiAA.L
	movlw	m2AA.H
	movwf	loAA.H
	movlw	m2AA.L
	movwf	loAA.L
	goto	comp.loDPZ
Tmore2:	movfw	T.H
	sublw	m1T.H
	skpc
	goto	Tmore1
	skpz
	goto	let1DPZ
	movfw	T.L
	sublw	m1T.L
	skpc
	goto	Tmore1
let1DPZ: movlw	m2T.H
	movwf	hiT.H
	movlw	m2T.L
	movwf	hiT.L
	movlw	m1T.H
	movwf	loT.H
	movlw	m1T.L
	movwf	loT.L
	movlw	m2AA.H
	movwf	hiAA.H
	movlw	m2AA.L
	movwf	hiAA.L
	movlw	m1AA.H
	movwf	loAA.H
	movlw	m1AA.L
	movwf	loAA.L
	goto	comp.loDPZ
Tmore1:	clrf	AA.H		; 1+) if T.H:T.L > 1T ; then AA=0
	clrf	AA.L
	bsf	Stsflg,AA.0	; set флаг AA.0
	goto	comp.toff
; 4) loDPZ=loT-T - период от измеренного до точки диапазона с меньшими оборотами
comp.loDPZ: bcf	Stsflg,AA.0	; clear флаг AA.0
	movfw	loT.H
	movwf	loDPZ.H
	movfw	loT.L
	movwf	loDPZ.L
	movf	T.L,W
	subwf	loDPZ.L,F
	btfss	STATUS,C
	decf	loDPZ.H,F
	movf	T.H,W
	subwf	loDPZ.H,F	; loDPZ.H:loDPZ.L готовы
; 5) xDPZ=loT-hiT - ширина диапазона периода, в которое уложился измеренный
	movfw	loT.H
	movwf	xDPZ.H
	movfw	loT.L
	movwf	xDPZ.L
	movf	hiT.L,W
	subwf	xDPZ.L,F
	btfss	STATUS,C
	decf	xDPZ.H,F
	movf	hiT.H,W
	subwf	xDPZ.H,F	; xDPZ.H:xDPZ.L готовы
; 6) hiDPZ=xDPZ-loDPZ - период от измеренного до точки диапазона с большими оборотами
; 6) hiDPZ=T-hiT
	movfw	T.H
	movwf	hiDPZ.H
	movfw	T.L
	movwf	hiDPZ.L
	movf	hiT.L,W		; не разрушается
	subwf	hiDPZ.L,F
	btfss	STATUS,C
	decf	hiDPZ.H,F
	movf	hiT.H,W
	subwf	hiDPZ.H,F	; hiDPZ.H:hiDPZ.L готовы
; самая главная формула мироздания!  ®одэрик
; 7) AA=(hiAA*loDPZ+loAA*hiDPZ)/xDPZ, comp AA.H:AA.L УОЗ, отступ до заднего фронта RearFront
; AA=( _A * _B  + _C * _D ) / _E
; AA=(hiAA*loDPZ+loAA*hiDPZ)/xDPZ

; A*B=AB
	MOVF	hiAA.L,W
	MOVWF	REG.AL
	MOVF	hiAA.H,W
	MOVWF	REG.AM
	MOVF	loDPZ.L,W
	MOVWF	REZ.L
	MOVF	loDPZ.H,W
	MOVWF	REZ.M

	CALL	MUL2B2B

	MOVF	RES.L,W
	MOVWF	AB.L
	MOVF	RES.M,W
	MOVWF	AB.M
	MOVF	RES.H,W
	MOVWF	AB.H
	MOVF	RES.U,W
	MOVWF	AB.U
;C*D=CD
	MOVF	loAA.L,W
	MOVWF	REG.AL
	MOVF	loAA.H,W
	MOVWF	REG.AM
	MOVF	hiDPZ.L,W
	MOVWF	REZ.L
	MOVF	hiDPZ.H,W
	MOVWF	REZ.M

	CALL	MUL2B2B

	MOVF	RES.L,W
	MOVWF	CD.L
	MOVF	RES.M,W
	MOVWF	CD.M
	MOVF	RES.H,W
	MOVWF	CD.H
	MOVF	RES.U,W
	MOVWF	CD.U
;AB+CD=CD
	MOVF	AB.L,W
	ADDWF	CD.L,F
	MOVLW	.1
	BTFSC	STATUS,C
	ADDWF	CD.M,F
	BTFSC	STATUS,C
	ADDWF	CD.H,F
	BTFSC	STATUS,C
	ADDWF	CD.U,F
	BTFSC	STATUS,C
	ADDWF	CD.UL,F

	MOVF	AB.M,W
	ADDWF	CD.M,F
	MOVLW	.1
	BTFSC	STATUS,C
	ADDWF	CD.H,F
	BTFSC	STATUS,C
	ADDWF	CD.U,F
	BTFSC	STATUS,C
	ADDWF	CD.UL,F

	MOVF	AB.H,W
	ADDWF	CD.H,F
	MOVLW	.1
	BTFSC	STATUS,C
	ADDWF	CD.U,F
	BTFSC	STATUS,C
	ADDWF	CD.UL,F

	MOVF	AB.U,W
	ADDWF	CD.U,F
	BTFSC	STATUS,C
	INCF	CD.UL,F
;CD/E
	MOVF	CD.L,W
	MOVWF	REZ.L
	MOVF	CD.M,W
	MOVWF	REZ.M
	MOVF	CD.H,W
	MOVWF	REZ.H
	MOVF	CD.U,W
	MOVWF	REZ.U
	MOVF	CD.UL,W
	MOVWF	REZ.UL
	CALL	DIV5B2B

;===================================================================================
; comp.toff:
; 1) comp toff.H:toff.L = T.H:T.L - AA.H:AA.L - ton.H:ton.L
; 2) let  toff.H = toff.H:toff.L/FF (>>8) 256 - макс Коэф деления предделителя, toff.H:toff.L сохран
; 3) pTMRoff = comp wghtMSB вес бита, след за старш ед битом в toff.H -> OPTION:0:[PS2:0]
; 4) TMRoff = FF-toff.H:toff.L/(вес позиции+1 MSB) где wghtMSB = закодированный Коэф в OPTION:0:[PS2:0], toff.H:toff.L сохран

; 1) comp toff.H:toff.L = T.H:T.L - AA.H:AA.L - ton.H:ton.L
; 1a) toff=T-AA
comp.toff: movfw T.H
	movwf	toff.H
	movfw	T.L
	movwf	toff.L
	movf	AA.L,W		; не разрушаются
	subwf	toff.L,F
	btfss	STATUS,C
	decf	toff.H,F
	movf	AA.H,W
	subwf	toff.H,F	; toff.H:toff.L=T-AA
; 1b) toff=toff-ton
	movf	ton.L,W		; не разрушаются
	subwf	toff.L,F
	btfss	STATUS,C
	decf	toff.H,F
	movf	ton.H,W
	subwf	toff.H,F	; toff.H:toff.L готов
; 2) let  toff.H = toff.H:ton.L/FF (>>8) 256 - макс Коэф деления предделителя TMR0=TMRA
; 3) pTMRoff = wghtMSB вес бита, след за старш ед битом в ton.H -> OPTION:0:[PS2:0]
; ®одэрик
	movf	toff.H,W	; toff.H = toff.H:toff.L/FF (>>8), toff.H:toff.L сохран
	movwf	Rtmp2
ip2:	movlw	0x07		; заготовка 0111 для OPTION:0:PS[2:0]
	movwf	Rtmp1
ip2.it1: btfsc	Rtmp2,7		; проверка старшего бита
	goto	ip2.rez	
	rlf	Rtmp2,F		; выдвижение следующего бита ?????????????????????
	decfsz	Rtmp1,F
	goto	ip2.it1
ip2.rez: movf	Rtmp1,W
	movwf	pTMRoff		; pTMRoff = OPTION:00000:PS[2:0] готовый предделитель
; 4) TMRoff = FF-toff.H:toff.L/(вес MSB + 1), ton.H:ton.L сохран
; деление на 256 128.....4 2
	movf	toff.H,W
	movwf	Rtmp2
	movf	toff.L,W	; toff.H:toff.L сохран
	movwf	Rtmp1
	incf	pTMRoff,W	; кол-во сдвигов соответственно значению делителя OPTION:0:[PS2:0] + 1
div.toff: BCF	STATUS,C
	RRF	Rtmp2,F
	RRF	Rtmp1,F
	addlw	0xff		; decrement W
	btfss	STATUS,Z
	goto	div.toff
	movf	Rtmp1,W		; большое вход значение - много сдвигов, малое - мало
	sublw	0xff		; TMRA считает от 0 -> FF
	movwf	TMRoff		; TMRoff готовый

	goto	ScanD

; Dividend - REZ.UL:REZ.U:REZ.H:REZ.M:REZ.L
; Divisor  - xDPZ.H:xDPZ.L
; Temp: Counter  - Q.REG.H
;       Remainder- Q.REG.M:Q.REG.L
; Out: REZ.H:AA.H:AA.L
DIV5B2B: movfw	REZ.M		; mia culpa
	movwf	AA.H
	movfw	REZ.L
	movwf	AA.L		; mia culpa
	CLRF	Q.REG.M
	CLRF	Q.REG.L
	MOVLW	.40
	MOVWF	Q.REG.H
LOOPU2416: RLF	AA.L,W
	RLF	AA.H,F
	RLF	REZ.H,F
	RLF	REZ.U,F
	RLF	REZ.UL,F
	RLF	Q.REG.L,F
	RLF	Q.REG.M,F
	RLF	AA.L,F
	MOVF	xDPZ.L,W
	SUBWF	Q.REG.L,F
	MOVF	xDPZ.H,W
	BTFSS	STATUS,C
	INCFSZ	xDPZ.H,W
	SUBWF	Q.REG.M,F

	BTFSC	STATUS,C
	BSF	AA.L,0
	BTFSC	AA.L,0
	GOTO	UOK46LL

	ADDWF	Q.REG.M,F
	MOVF	xDPZ.L,W
	ADDWF	Q.REG.L,F
UOK46LL: DECFSZ	Q.REG.H,F
	GOTO	LOOPU2416
	RETURN

;IN:REG.AM,REG.AL; REZ.M,REZ.L
;OUT: RES.U,RES.H,RES.M,RES.L
MUL2B2B: CLRF	RES.U
	CLRF	RES.H
	CLRF	RES.M
	MOVLW	0x80
	MOVWF	RES.L
NEXTBIT: RRF	REG.AM,F
	RRF	REG.AL,F
	BTFSS	STATUS,C
	GOTO	NOBIT_L
	MOVF	REZ.L,W
	ADDWF	RES.M,F
	MOVF	REZ.M, W
	BTFSC	STATUS,C
	INCFSZ	REZ.M, W
	ADDWF	RES.H, F
	BTFSC	STATUS,C
	INCF	RES.U, F
	BCF	STATUS,C
NOBIT_L: BTFSS	REG.AL,7
	GOTO	NOBIT_H
	MOVF	REZ.L,W
	ADDWF	RES.H,F
	MOVF	REZ.M, W
	BTFSC	STATUS,C
	INCFSZ	REZ.M, W
	ADDWF	RES.U,F
NOBIT_H: RRF	RES.U,F
	RRF	RES.H,F
	RRF	RES.M,F
	RRF	RES.L,F
	BTFSS	STATUS,C
	GOTO	NEXTBIT
	RETURN

; Вкл бобины для накопления энергии в зависимости от активного цилиндра Dact D1=0 || D2=0
; Установка ICflg
ICon:	btfsc	Dact,D2
	goto	IC1on
	movlw	0x41		; веса IC2 + TAHO, 01000001 (1 << IC2) + 1
	movwf	PORTB		; IC2on:
	goto	setICflg
IC1on:	movlw	0x20		; вес IC1 00100000 (1 << IC1)
	movwf	PORTB
setICflg: bsf	Stsflg,ICflg
	return

; Выкл бобин всех скопом, РАЗРЯД!, сброс ICflag
ICoff:	clrf	PORTB
	bcf	Stsflg,ICflg
	return

; Rtmp0=Cycles/256*3
delay.ms: movlw	delay.cycle
	movwf	Rtmp0
	clrf	Rtmp1
dloop:	decfsz	Rtmp1,f
	goto	recurs
	decfsz	Rtmp0,f
recurs:	goto	dloop
	return

;sboj:	fill (goto Init),__CODE_END-$
	end
