   	TITLE		"16F870 mobile Decoder w/EMF feedback Program"
;			
;
;
;----------------------------------------------------------------------
; Change history (check also in sourcesafe)
;----------------------------------------------------------------------

;		
; 17/05/02	Added fixes to DCC->analog. Now tests and if speed is 0 will allow periodic WDT to revise direction.
;		RP9.2.4B working correctly  (speed control in DC poor but runs OK as far as direction goes)
;		If DC is 'marginal' i.e. very slow, it sometimes doesn't reverse.
; 18/05/02	This is the original int211 but with Gil's mods for the 'drop out' problem 
;		Works correctly.
;		Page mode problem fixed
;		EMF_control and integral changed to latest version
;		Works fine. Added my speed table and CV defaults. Not to RP recommendations but
;		optimised for can motors.
;		Save as dec132. This is the RP9.2.4 compliant version. Analog running in pure DC only
;		Analog can be disabled with CV29. 
; 03/09/02	Changes to hard reset and action on setting CV1.
;		Fixed jump table problem
;		Now version 132 rev b
; 05/11/02	Dec133. Modified version of Dec132 for analog switch
;		using CV54 bit 7. (Looks like Dec131 in this mode)
;		Also added Gil's change in integral. x16 below speed
;		.15
;		Tested and working. 
; 24/11/02	Fix to CV29 default when changing CV1. Seems OK now.

;----------------------------------------------------------------------


;
; Assembly options
	LIST	P=16F870,r=hex,N=75,C=120,T=ON
;#DEFINE	TEST_MODE
#DEFINE	MBINT
;
;
	include		"p16f870.inc"
	__IDLOCS	h'0001'
	__CONFIG	h'3D76' ; CP off,DEBUG off,WRT off,CPD off,LVP off,BOD on,PUTimer on, WDT on, HS clock

;-----------------------------------------
; constants and mem locations	

; Controller conversion point from PDFF (PI+) to PI, to prevent overflow

KFR_LIMIT  equ	d'50'


; CV7 and CV8 implemented as constants to save EEPROM space
;
VERSION_VAL	equ	d'133'
MAN_ID_VAL	equ	d'13'
Version		equ	h'F6'	; CV special mark (number -1)
ManId		equ	h'F7'

;
; FLG == |LV-7|VP-6|VB-5|LS-4|H-3|SC-0:2|
;
LV	equ	7		; Last bit value
VP	equ	6		; Valid Packet
VB	equ	5		; Valid Byte
H	equ	3		; Half-Bit
H_msk	equ	b'00001000'	; mask of half-bit
LS	equ	4		; Last sample
SC_msb	equ	2		; sample count MSB
LS_msk	equ	B'00010000'	; mask for last sample
;

;-----------------------------------------------
; FLG2 = POL|AG|DG|DIRF|DIRC|SM2|SM|AM
AM		equ	0		; Analog Mode
AM_msk		equ	b'00000001'
SM		equ	1		; sample flag
SM2		equ	2		; sample lock
DIRC		equ	3		; direction change slow-down
DIRF		equ	4		; current direction 
DIRF_msk	equ	b'00010000'	
DG		equ	5		; digital mode
DG_msk		equ	b'00100000'
AG		equ	6		; digital following analog (unidirectional)
AG_msk		equ	b'01000000'
POL		equ	7		; DC polarity changed
POL_msk		equ	b'10000000'
;-----------------------------------------------
; pwmmode = |DCmode|0|HF|EMF|sw0-sw3
PWM_HF		equ	5	; High freq. PWM
PWM_HF_msk	equ	b'00100000'
PWM_EMF		equ	4	; enable feedback
PWM_EMF_msk	equ	b'00010000'
				; sw0-sw3 speed step below of which to switch to low freq pwm
PWM_DCmode	equ	7
PWM_DCmode_msk	equ	b'10000000'
;-----------------------------------------------
; Effect bits
;
EF_SPD		equ	7	; Speed related counter (by pwm)
EF_SPD_msk	equ	b'10000000'
EF_QA		equ	6	; Qtr Sec phase A
EF_QA_msk	equ	b'01000000'
EF_QB		equ	5	; Qtr Sec phase B
EF_QB_msk	equ	b'00100000'
EF_Fwd		equ	4	; Fwd ON
EF_Fwd_msk	equ	b'00010000'
EF_Rev		equ	3
EF_Rev_msk	equ	b'00001000'
EF_MARs		equ	2	; Off-Dim-On-Dim-
EF_MARs_msk	equ	b'00000100'
EF_ST		equ	1	; Strobe Light
EF_ST_msk	equ	b'00000010'
EF_Dim		equ	0
EF_Dim_msk	equ	b'00000001'

;-----------------------------------------------
; Logical Function control bits
;
#DEFINE		FN_NUM	5	; set this to the total no. of functions (including fl/rl)

FN_FL		equ	0
FN_RL		equ	1
FN_F1		equ	2
FN_F2		equ	3
FN_F3		equ	4
FN_F4		equ	5
FN_F5		equ	6
FN_RST		equ	7

OUT1		equ	0
OUT1_msk	equ	b'00000001'
OUT2		equ	1
OUT2_msk	equ	b'00000010'
OUT3		equ	2
OUT3_msk	equ	b'00000100'
OUT4		equ	3
OUT4_msk	equ	b'00001000'
OUT5		equ	4
OUT5_msk	equ	b'00010000'
OUT6		equ	5
OUT6_msk	equ	b'00100000'
OUT7		equ	6
OUT7_msk	equ	b'01000000'
OUT8		equ	7
OUT8_msk	equ	b'10000000'

;-----------------------------------------------
; PIO bits
;
;  The following definitions should be changed 
;  according to pin assignments   
;
#ifdef	MBINT

#DEFINE		BEMF_port	PORTA
IN_BEMF		equ	0
IN_BEMF_msk	equ	b'00000001'

;
; Set the following mask as a combination of all the outputs controlled by logical Fn's in PORTA
;
PORTA_FN_msk		equ	0
PORTA_Input_msk 	equ	IN_BEMF_msk
PORTA_In_Analog_msk	equ	IN_BEMF_msk
PORTA_ON_Analog_msk	equ	0
;
;
; 
;
#DEFINE		IN_OverVolt_port	PORTB
IN_OverVolt	equ	0	; Over-voltage or Marklin analog detection circuit
IN_OverVolt_msk equ	b'00000001'
#DEFINE		OUT1_port	PORTB
OUT1_pad	equ	5	; forward light
OUT1_pad_msk	equ	b'00100000'
#DEFINE		OUT2_port	PORTB
OUT2_pad	equ	4	; reverse light
OUT2_pad_msk	equ	b'00010000'
#DEFINE		OUT3_port	PORTB
OUT3_pad	equ	3	; Function 1
OUT3_pad_msk	equ	b'00001000'
#DEFINE		OUT4_port	PORTB
OUT4_pad	equ	2	; Function 2
OUT4_pad_msk	equ	b'00000100'
#DEFINE		OUT5_port	PORTB
OUT5_pad	equ	1	; Function 3
OUT5_pad_msk	equ	b'00000010'
#DEFINE		IN_pgmclk_port	PORTB
IN_pgmclk	equ	6	; serial programming clk
IN_pgmclk_msk	equ	b'01000000'
#DEFINE		IN_pgmdat_port	PORTB
IN_pgmdat	equ	7	; serial programming data
IN_pgmdat_msk	equ	b'10000000'


;
; Set the following mask as a combination of all the outputs controlled by logical Fn's in PORTB
;
PORTB_FN_msk	equ	OUT1_pad_msk | OUT2_pad_msk | OUT3_pad_msk | OUT4_pad_msk | OUT5_pad_msk
PORTB_Input_msk equ	IN_OverVolt_msk | IN_pgmclk_msk | IN_pgmdat_msk
PORTB_In_Analog_msk equ	IN_OverVolt_msk |IN_pgmclk_msk |IN_pgmdat_msk |OUT3_pad_msk |OUT4_pad_msk |OUT5_pad_msk
PORTB_ON_Analog_msk equ	OUT1_pad_msk | OUT2_pad_msk


#DEFINE		OUT_PWM_port	PORTC
OUT_PWM		equ	2	; pwm signal
OUT_PWM_msk	equ	b'00000100'
#DEFINE		OUT_fwd_port	PORTC
OUT_fwd		equ	5	; forward dir
OUT_fwd_msk	equ	b'00100000'
#DEFINE		OUT_back_port	PORTC
OUT_back	equ	6	; backward dir
OUT_back_msk	equ	b'01000000'
#DEFINE		IN_port		PORTC
IN_bit		equ	4	; rail input pin
IN_bit_msk	equ	b'00010000'
IN_bit2		equ	7	; second rail input
IN_bit2_msk	equ	b'10000000'


Motor_Dmp_msk	equ	OUT_fwd_msk | OUT_back_msk
;
; Set the following mask as a combination of all the outputs controlled by logical Fn's in PORTC
;
PORTC_FN_msk		equ	0
PORTC_Input_msk		equ	IN_bit_msk | IN_bit2_msk
PORTC_In_Analog_msk	equ	IN_bit_msk | IN_bit2_msk
PORTC_ON_Analog_msk	equ	OUT_PWM_msk




;
; Set the following mask with 1's in every position where an output is active low
;
Out_Active_Low	equ	0
;
#endif

#ifndef MBINT

#DEFINE		BEMF_port	PORTA
IN_BEMF		equ	0
IN_BEMF_msk	equ	b'00000001'

;
; Set the following mask as a combination of all the outputs controlled by logical Fn's in PORTA
;
PORTA_FN_msk		equ	0
PORTA_Input_msk 	equ	IN_BEMF_msk
PORTA_In_Analog_msk	equ	IN_BEMF_msk
PORTA_ON_Analog_msk	equ	0
;
;
; 
;
#DEFINE		IN_OverVolt_port	PORTB
IN_OverVolt	equ	0	; Over-voltage or Marklin analog detection circuit
IN_OverVolt_msk equ	b'00000001'
#DEFINE		OUT1_port	PORTB
OUT1_pad	equ	5	; forward light
OUT1_pad_msk	equ	b'00100000'
#DEFINE		OUT2_port	PORTB
OUT2_pad	equ	4	; reverse light
OUT2_pad_msk	equ	b'00010000'
#DEFINE		OUT3_port	PORTB
OUT3_pad	equ	3	; Function 1
OUT3_pad_msk	equ	b'00001000'
#DEFINE		OUT4_port	PORTB
OUT4_pad	equ	2	; Function 2
OUT4_pad_msk	equ	b'00000100'
#DEFINE		OUT5_port	PORTB
OUT5_pad	equ	1	; Function 3
OUT5_pad_msk	equ	b'00000010'
#DEFINE		IN_pgmclk_port	PORTB
IN_pgmclk	equ	6	; serial programming clk
IN_pgmclk_msk	equ	b'01000000'
#DEFINE		IN_pgmdat_port	PORTB
IN_pgmdat	equ	7	; serial programming data
IN_pgmdat_msk	equ	b'10000000'


;
; Set the following mask as a combination of all the outputs controlled by logical Fn's in PORTB
;
PORTB_FN_msk	equ	OUT1_pad_msk | OUT2_pad_msk | OUT3_pad_msk | OUT4_pad_msk | OUT5_pad_msk
PORTB_Input_msk equ	IN_OverVolt_msk | IN_pgmclk_msk | IN_pgmdat_msk
PORTB_In_Analog_msk equ	IN_OverVolt_msk |IN_pgmclk_msk |IN_pgmdat_msk |OUT3_pad_msk |OUT4_pad_msk |OUT5_pad_msk
PORTB_ON_Analog_msk equ	OUT1_pad_msk | OUT2_pad_msk


#DEFINE		OUT_PWM_port	PORTC
OUT_PWM		equ	2	; pwm signal
OUT_PWM_msk	equ	b'00000100'
#DEFINE		OUT_fwd_port	PORTC
OUT_fwd		equ	1	; forward dir
OUT_fwd_msk	equ	b'00000010'
#DEFINE		OUT_back_port	PORTC
OUT_back	equ	3	; backward dir
OUT_back_msk	equ	b'00001000'
#DEFINE		IN_port		PORTC
IN_bit		equ	0	; rail input pin
IN_bit_msk	equ	b'00000001'
IN_bit2		equ	7	; second rail input
IN_bit2_msk	equ	b'10000000'


Motor_Dmp_msk	equ	OUT_fwd_msk | OUT_back_msk
;
; Set the following mask as a combination of all the outputs controlled by logical Fn's in PORTC
;
PORTC_FN_msk		equ	0
PORTC_Input_msk		equ	IN_bit_msk | IN_bit2_msk
PORTC_In_Analog_msk	equ	IN_bit_msk | IN_bit2_msk
PORTC_ON_Analog_msk	equ	OUT_PWM_msk


;
; Set the following mask with 1's in every position where an output is active low
;
Out_Active_Low	equ	0
;

#endif

;------------------------
; Timing constants
;
FREQ		equ	d'16'	; MHz (16, 20, 24)
SAMPLE_PRD	equ	d'44'	; 44 usec
PWM_CNT_PRESET	equ	d'80'	; the number of 44usec cycles to increment/decrement PWM threshold (256 increments) 
				;    - based on formula t(incr) = (CV#3 + CV#23) * t = [(CV#3 + CV#23) * .896] / 256
				;    - PWM_CNT_PRESET = t / 44us =  .896s / (256 * 44us)
CHAF_PRESET_CNT	equ	d'255'	; 255 * 80 * 44 == .9 sec
SVC_TO_PRESET	equ	d'6'	;  6 * 80 * 44 ==  20 ms
STEP_MULTIPLIER equ	d'151'  ; 255*16/151 = 27
PREAMBLE_CNT	equ	d'18'	; counter for min. preamble half-bits (for sync).

;------------------------
; Command bits
;
CMD_dir		equ	5
CMD_dir_msk	equ	b'00100000'		
;-------------------------------------
; volatile memory
;
	CBLOCK	0x0020
; PWM variables
		PWM_step	; current step256				20
		PWM_cnt		; counter					21
		PWM_step_tgt	; for accel/decel effect			22
		PWM_thr		; current threshold				23				
		PWM_Accel_cnt	; counts the 44us cycles between accel updates	24
		PWM_timer	; contents of CV3 or CV4, decremented to 0	25
		TStep		; time of speed step in usec			26
		PWM_t		; accumulated time of step			27
		state		; state machine state				28
		SVC_timer	; expiration of svc mode			29
		Effect_cnt	; rolls ~every sec				2a
;---------------------------------------------------------------------------------
;	.5s | .25sA | .25sB | Fwd | Rev | Mars | Strb | Dim
;         00000000 - On/Off all the time.
		ON_Effect	; mask for special effects			2b
;
; Byte variables
		Buf		; byte buffer   addr				2c
		B_cnt		; counter for packet length			2d	
		Err		; error byte					2e
; ----- Addr bytes ------
		Addr2_b		; cover extended addressing			2f
		Addr_b		; packet bytes - address			30
; ----- Instruction bytes + error byte -------
		Cmd_b		; " - command					31
		Chk_b		; " - check					32
		Sup_1		; long instructions				33
		Sup_2		;						34
		LastAddr2_b	;						35
		LastAddr_b	;						36
		LastCmd_b	;						37
		LastChk_b	;						38
		LastSup1	;						39
		LastSup2	;						3a
;---------------------------------------------------
; Configuration Vars
;

		FLG		; Flag word (byte)				3b
		FLG2		; second Flag					3c
; volatile memory

		MEM_FILE_S:0	; start of file
		Active_addr	; stores the active address of decoder		3d
		Decoder_addr	; -"- decoder original addr			3e
		Arg1		; general purpose args				3f
		Arg2		;						40
		Arg3		;						41						
		Arg4		;						42
		Arg5		;						43
		Arg6		;						44
		DEC_STATUS	; Decoder status word				45
		Page_low	; Register paging - 53,54			46
		Page_high	;						47
		B_rem		; Bytes roll counter				48
		Last_spd	; Saved speed for half-steps			49
		Active_addr_x	; extended addressing				4a
		Decoder_addr_x	; -"- decoder original addr			4b
		DEC_config	; ram copy of Config_data_1 (CV29)		4c		
		Fn_control	; master on/off of functions			4d
		Sec_cnt		; T/O counter (seconds)				4e
		pwmmode		; pwm mode byte					4f
		magic		; reset condition magic byte			50
		Chaf_cnt	; for speed-dependent functions (sound)		51
		EEAddrBuf	; buffer for EE address				53
		EMFcutoff	; RAM copy of CV10				54
		PWM_val		; calculated desired PWM value			55
		Integ		; integral accumulator of BEMF			56
		PWM_step_tg1	; temporary target speed for chg direction	57
		EMF_Ki		; controller constants				58
		EMF_Kp		;						59
		EMF_Kfr		;						5a
		EMF_val		; last sampled value of BEMF			5b
		Vmin		;						5c	
		Vmax		;						5d
		EEdata_temp	; temp. store for EE written data		5e
	ENDC
	CBLOCK 	0x070
		_w		; save place for W				70
		_status		; " - status					71
		_pclath		; " - pclath					72
	ENDC	
;-------------------------------------
; Decoder status bits
; |SYNC|BP|DP|CR|NG|AC|CO|SV|
DEC_STAT_SV		equ	0	; Service mode bit
DEC_STAT_SV_msk		equ	b'00000001'
DEC_STAT_CO		equ	1	; Consist Control Active bit
DEC_STAT_CO_msk		equ	b'00000010'	; mask
DEC_STAT_AC		equ	2	; sent to consist addr
DEC_STAT_AC_msk		equ	b'00000100'
DEC_STAT_NG		equ	3	; negative number (for spline)
DEC_STAT_NG_msk		equ	b'00001000'
DEC_STAT_CR		equ	4	; Consist reverse bit
DEC_STAT_CR_msk		equ	b'00010000'
DEC_STAT_DP		equ	5	; Dup (repeat) packet
DEC_STAT_DP_msk		equ	b'00100000'
DEC_STAT_BP		equ	6	; Begin of packet
DEC_STAT_BP_msk		equ	b'01000000'
DEC_STAT_SYNC		equ	7	; SYNC mode
DEC_STAT_SYNC_msk	equ	b'10000000'
;-------------------------------------
; Decoder Config bits
; |DT|0|AA|ST|AK|PW|FL|DR|
DEC_CFG_DR		equ	0	; Direction. 0=Normal 1=Reversed
DEC_CFG_FL		equ	1	; Lights Control Bit. 0=in SpeedDir instruction 1=bit 4 of Fn group 1
DEC_CFG_PW		equ	2	; Analog Power conversion control
DEC_CFG_AK		equ	3	; Advanced Ack
DEC_CFG_ST		equ	4	; Speed table / polynomial
DEC_CFG_AA		equ	5	; Extended Addressing
DEC_CFG_DT		equ	7	; Decoder Type. 0=multifunction 1=accessory
;-------------------------------------
; EEPROM Data 
;
;****** warning: locations in this list are critical.
;****** add vars at the end only
;
	CBLOCK	h'0'		; EEPROM addresses

		PRIM_Addr	;CV1  = 0
		V_Start		;CV2
		ACC_Rate	;CV3
		DEC_Rate	;CV4
		V_High		;CV5
		V_Mid		;CV6
;		Version		;CV7   **** implemented as constants 
;		ManId		;CV8
		PWMTot		;CV9  = 6
		EMFCut		;CV10
		Packet_TO	;CV11 = 8
; CV14-16 reserved
		ExtAddr2	;CV17 = 9 MSB
		ExtAddr1	;CV18
		Consist_addr	;CV19 = 11
; CV20 reserved
		CAct_F1_F8	;CV21 = 12
		CAct_LMP	;CV22
		Accel_Adj	;CV23
		Decel_Adj	;CV24
		CABSPD_Step	;CV25 = 16
; CV26-28 reserved
		Config_data_1	;CV29 = 17
		Err_Info	;CV30 = 18
;
		FL_loc		;CV33 = 19
		RL_loc		;CV34 = 20
		F1_loc		;CV35 = 21
		F2_loc		;CV36 = 22
		F3_loc		;CV37 = 23
;
		FL_Effect	;CV49 = 24
		RL_Effect	;CV50 = 25
		F1_Effect	;CV51 = 26
		F2_Effect	;CV52 = 27
		F3_Effect	;CV53 = 28
		PWM_Mode	;CV54 = 29
		Ki		;CV55 = 30
		Kp		;CV56 =	31
		Kfr		;CV57 = 32
		Reserv1		;CV58 = 33
		Reserv2		;CV59 = 34
		Reserv3		;CV60 = 35
		
		SPD_Tbl:1C	;CV67 - 94 = 36 - 63 (28 values)
		MEM_FILE_E	; end of file
	ENDC
	
	
	





;------------------------------------
; code starts here
;
		org 0
powerup		clrf	PCLATH
		clrf	Page_high
		clrf	Page_low
		goto	init_start

		org 4
;----------------------------------------
; Interrupt every 22us during sync,
; then drop rate to 1/44us
;

Int		movwf	_w					;0
		swapf	STATUS,	w				;1
		clrf	STATUS					;2
		movwf	_status					;3
		movf	PCLATH, w				;4
		movwf	_pclath					;5
		clrf	PCLATH					;6
		bcf	INTCON, T0IF				;
		movlw	d'256' + d'14' - (SAMPLE_PRD * FREQ / 4);7
		btfsc	DEC_STATUS, DEC_STAT_SYNC		;8
		addlw	SAMPLE_PRD * FREQ / 8				;9
		movwf	TMR0			; restart count	;10
; pwm
		btfss	DEC_STATUS, DEC_STAT_SYNC		;11
		goto	l_Int_1					;12
		movlw	H_msk					;13   in sync mode for pwm, divide sample rate by 2 
		xorwf	FLG,		F			;14
		btfss	FLG,		H			;15
		goto	l_Int_2
;
l_Int_1		incf	PWM_Accel_cnt, 	F			;  This is handled in pwm routine
		movlw	SAMPLE_PRD 				; 
		addwf	PWM_t,	F				;  add time in us
;
l_Int_2		movf	FLG,		W			; * The following change made to allow any pin/port as input
		btfsc	IN_port,	IN_bit			
		xorlw	LS_msk
		andlw	LS_msk
		btfss	STATUS,		Z			; sample == last sample
		goto	l_Int_state
		btfss	FLG,		SC_msb			; count up to 4 and stay at 4
		incf	FLG,		F	
		call	Int_pwm
		goto	Int_end
;
; state machine dispatch
;
l_Int_state	btfsc	IN_port,	IN_bit			; if rising edge then no need for further checking
		goto	l_Int_4
		btfss	IN_port,	IN_bit2			; distinguish PWM DC form DCC
		goto	l_Int_3
l_Int_4		clrwdt						; cleared only if signal is arriving
		bsf	FLG2,		POL
		xorwf	FLG,		F
l_Int_3		movf	state,		W
		addwf	PCL,		F
		goto	l_state_sync0				; reset sync
		goto	l_state_sync1
		goto	l_state_sync2
		goto	l_state_sync3
		goto	l_state_hbit
;		goto	l_state_bit
;
l_state_bit	call	Int_pwm
		bcf	FLG,		LV					;
		btfss	FLG,		SC_msb	; is samp no. >= 4		;
		bsf	FLG,		LV
		call	byte
		addwf	PCL,		F
		goto	l_state_sync0		; ret w==0 for error
		decf	state,		F	; next wait for hbit
		goto	l_clr_count	


;---------------------------------------------
;
; calculate MARs
MARs    	swapf   Effect_cnt ,	w  
        	andlw   0x0F            
        	addwf   PCL,	f           

        	retlw   0x0F           
        	retlw   0x07
        	retlw   0x07
        	retlw   0x03
        	retlw   0x03
        	retlw   0x01
        	retlw   0x01
        	retlw   0x00
        	retlw   0x00
        	retlw   0x01
        	retlw   0x01
        	retlw   0x03
        	retlw   0x03
        	retlw   0x07
        	retlw   0x07
        	retlw   0X0F

;
;-----------------------------------------
;  Decoder control instructions
;
decod_ctl	btfsc	Cmd_b,	4
		goto	consist_ctl
		movlw	h'f'
		andwf	Cmd_b,	W	; get low nibble
		addwf	PCL,F		; jump table
		goto	start		; 0000 decoder reset
		goto	hard_reset	; 0001 hard reset
		goto 	l_roll		; 0010 reserved
		goto	l_roll		; 0011  "
		goto	l_roll		; 0100  "
		goto	l_roll		; 0101  "
		goto	clr_adv_ack	; 0110  adv ack not supported
		goto	set_adv_ack	; 0111   "
		goto	l_roll		; 1000  reserved
		goto	l_roll		; 1001  "
		goto	clr_adv_addr	; 1010  ext address
		goto	set_adv_addr	; 1011  "
		goto	l_roll		; 1100  reserved
		goto	l_roll		; 1101  "
		goto	l_roll		; 1110  "
		goto	ack		; 1111 acknowledge (adv ack n/a)
;--------------------------------
; main dispatcher
;
l_disp_2	movf	Addr_b,		W
		xorwf	Decoder_addr, 	W	; is it Decoder addr in consist mode
		btfss	STATUS,		Z	;
		goto	l_disp_svc
		movf	Addr2_b, 	W
		xorwf	Decoder_addr_x, W
		btfss	STATUS, 	Z
		goto	l_disp_svc
		movf	Cmd_b,		W
		andlw	b'11000000'		; is cmd == 10x (function group 1-2)
		xorlw	b'10000000'
		btfsc	STATUS,	Z
		goto	l_match
;
l_disp_svc	movf	Addr2_b,	W  ; test for ext. byte == 0
		btfss	STATUS,		Z
		goto	p_delay_start	
		movlw	h'F0'
		andwf	Addr_b,	W
		xorlw	h'70'		; test for service mode
		btfss	STATUS,	Z	; if Addr_b == 0x70 (C==1) go to service mode
		goto	p_delay_start	; no match, release packet buffer
		goto	svc_mode
;-------------------------------------------

; dispatch- main entry
;
l_dispatch	btfss	FLG2,		AM	; if analog mode and packet detected, reset!	
		goto	l_disp_3
		bcf	FLG2,	AM
		bsf	FLG2,	AG		; mark digital following analog
		bsf	STATUS, RP0
		call	set_power		; reset power, I/O and A/D				
		clrf	PWM_step_tgt
		call	pwm_setd_start		; reset direction and target spd
		clrf	PWM_step
		call	splineIt
		call	EMF_control
;
l_disp_3	bcf	DEC_STATUS,	DEC_STAT_AC
		movf	Addr_b, 	W
		iorwf	Addr2_b,	W
		btfsc	STATUS,		Z	; if addr == broadcast
		goto	l_match
		movf	Addr_b,		W
		xorwf	Active_addr, 	W	; is it the active addr (decoder/consist)?
		btfss	STATUS,		Z
		goto	l_disp_2
		movf	Addr2_b, 	W
		xorwf	Active_addr_x, 	W	; support ext. addr.it
		btfss	STATUS, 	Z
		goto	l_disp_2
		btfsc	DEC_STATUS, 	DEC_STAT_CO
		bsf	DEC_STATUS,	DEC_STAT_AC
; address match
l_match		clrf	Sec_cnt		; clear the Timeout counter
		movlw	SVC_TO_PRESET	; preset the SVC mode timer
		movwf	SVC_timer
		call	isRepeatPacket	; is it a dup packet ?
		movf	Cmd_b,	F	; test if reset - if not, clear service mode bit
		btfss	STATUS,	Z
		bcf	DEC_STATUS, DEC_STAT_SV
;
l_parse		clrf	B_rem
		incf	B_rem,	F	; count first byte to roll
		swapf	Cmd_b,	F	; get major opcode
		rrf	Cmd_b,	W
		swapf	Cmd_b,	F
		andlw	h'07'
		addwf	PCL,F		; jump Table
		goto	decod_ctl	; 000 decoder control / consist control
		goto	adv_op		; 001 advanced op
		goto	speed_dir	; 010 speed & direction
		goto	speed_dir	; 011   "
		goto	func_1		; 100 Function group 1
		goto	func_2		; 101 Function group 2
		goto	p_sync_start	; 110 reserved
;		goto	CV_access_long	; 111 Config Var access, long form / short form
;----------------------------------------
; Direct access to CVs - long form
;
CV_access_long	incf	B_rem,	F
		btfsc	Cmd_b,	4
		goto	CV_access_short
;
l_cvsl_start	incf	B_rem,	F
		btfsc	DEC_STATUS, DEC_STAT_AC
		goto	l_roll			; not active if sent to consist address
		movf	Chk_b,	w		; get low CV# bits
		movwf	Arg1			
		movf	Cmd_b,	w
		andlw	b'00000011'		; get high CV# bits
		call	CV_to_addr
		movwf	EEAddrBuf
		xorlw	h'FF'			; check illegal CV#
		btfsc	STATUS, Z
		goto	l_roll
		movf	Sup_1,	w
		movwf	Chk_b			; make same as CV access short
		rrf	Cmd_b,	f		
		rrf	Cmd_b,	w
		andlw	b'00000011'		; get cmd bits
		addwf	PCL,	f
		goto	l_roll			; reserved
		goto	l_cvsl_vf
		goto	l_cvsl_bit
		goto	l_cvsl_wr
;
l_cvsl_wr	btfss	DEC_STATUS,	DEC_STAT_DP	;test for repeat packet
		goto	l_roll			;if got here, do nothing and wait for repeat
		movf	Chk_b,	w
l_cvsl_wr1	call	cvwr_ok
		addwf	PCL,	f
		goto	l_roll
		goto	l_cvsl_wr2
		movf	EEdata_temp,	W	; write CV1
		call	ee_write
		movlw	Consist_addr		; clear consist address
		movwf	EEAddrBuf
		clrw
		call	ee_write  
		movlw	Config_data_1
		call	ee_read
		andlw   B'11011111'		;clear extended addressing 
		movwf	DEC_config
		call	write_config
		goto	ack
l_cvsl_wr2	movf	EEdata_temp,	W
		call	ee_write
		goto	ack
;
l_cvsl_vf1	movwf	EEAddrBuf
;
l_cvsl_vf	call	ee_read_1
		xorwf	Chk_b,	w
		btfsc	STATUS,	Z
		goto	ack
		goto	l_roll
;
l_cvsl_bit	movf	Chk_b,	w
		andlw	b'00000111'
		movwf	Arg4
		incf	Arg4,	f
		clrf	Arg3
		bsf	STATUS,	C
;
l_cvsl_lp1	rlf	Arg3,	f	; get a 1 bit into position in Arg3
		decfsz	Arg4,	f
		goto	l_cvsl_lp1
		call	ee_read_1
		movwf	Arg4
		btfsc	Chk_b,	4
		goto	l_cvsl_wbit
		movf	Arg3,	w	; load bit mask-> w
		andwf	Arg4,	w
		btfsc	Chk_b,	3	; if we test for 0, the value in w should be 0
		xorwf	Arg3,	w	; if we test for 1, invert the bit
		btfsc	STATUS,	Z
		goto	ack
		goto	l_roll
;	


;----------------------------------------
; Direct access to CVs - short form
;
CV_access_short	movf	Cmd_b,	w
		andlw	0x7
		addwf	PCL,	F
		goto	l_cvs_data	; 000 data reg 0
		goto	l_cvs_data	; 001 data reg 1
		goto	l_cvs_data	; 010 data reg 2
		goto	l_cvs_data	; 011 data reg 3
		goto	l_cvs_config	; 100 Config CV29
		goto	l_cvs_Page	; 101 page reg
		goto	l_cvsl_vf1	;l_cvs_Version	; 110 version # CV7
		goto	l_cvsl_vf1	;l_cvs_ID	; 111 manufacturer ID CV8
;
l_cvs_data	addwf	Page_low, W
		btfss	DEC_STATUS, DEC_STAT_SV		; are we in service mode
		addlw	d'20'				; make it CV#21 - 24 (std is 23,24)
		movwf	Arg1
		movf	Page_high,W
		call	CV_to_addr
		goto	l_cvs_op
;
l_cvs_config	movlw	Config_data_1	; modify basic config CV29 
		goto	l_cvs_op
;
l_cvs_Page	decf	Chk_b,	F	; Page--
		clrf	Arg1		; (Page# - 1) *4, high & low
		bcf	STATUS,	C
		rlf	Chk_b,	F
		rlf	Arg1,	F
		rlf	Chk_b,	F
		rlf	Arg1,	F
		btfss	Cmd_b,	3 	; page is in mem, not EEPROM
		goto	l_cvs_pg_vf
		movf	Chk_b,	W	; write value
		movwf	Page_low
		movf	Arg1,	W
		movwf	Page_high
		goto	ack
;
l_cvs_pg_vf	movf	Chk_b,	W
		xorwf	Page_low, W
		btfss	STATUS,	Z
		goto	l_roll
		movf	Arg1, W
		xorwf	Page_high, W
		btfss	STATUS,	Z
		goto	l_roll		; not verified
		goto	ack
	
;
l_cvs_op	movwf	EEAddrBuf
		xorlw	h'FF'
		btfsc	STATUS,	Z
		goto	l_roll
		btfss	Cmd_b,	3	; test op code
		goto	l_cvsl_vf
		goto	l_cvsl_wr

l_cvsl_wbit	btfss	DEC_STATUS,	DEC_STAT_DP
		goto	l_roll		;if got here, do nothing and wait for repeat
		movf	Arg3,	W	
		iorwf	Arg4,	W	; get the orig value and set bit to 1
		btfss	Chk_b,	3
		xorwf	Arg3,	W	; clear the bit
		goto	l_cvsl_wr1
;-------------------------------------------
; Service Mode
;
svc_mode	movlw	SVC_TO_PRESET	; preset the SVC mode timer
		movwf	SVC_timer
		bsf	DEC_STATUS,	DEC_STAT_SV
		clrf	Sec_cnt		; reset timeout counter
		call	isRepeatPacket	; only act on repeat packets
		btfss	DEC_STATUS,	DEC_STAT_DP
		goto	p_delay_start
		movf	Chk_b,	W
		movwf	Sup_1
		movf	Cmd_b,	W
		movwf	Chk_b
		movf	Addr_b,	W
		movwf	Cmd_b
		btfsc	B_cnt,	1	; is B_cnt <= 1
		goto	l_cvsl_start
		goto	CV_access_short
;-----------------------------------------


; validate CV write
cvwr_ok		movwf	EEdata_temp
		movlw	Version		; check if addr == CV7 or CV8 (read only)
		xorwf	EEAddrBuf, W
		andlw	0xFE		; mask bit 0
		btfsc	STATUS, Z
		retlw	0
		movf	EEAddrBuf, W
		sublw	ExtAddr2	;is it CV17 - ext address hi byte
		btfss	STATUS,	Z
		goto	l_cvwr_1 	; continue tests
		movlw	B'11000000'  	;0xC0 minimum
		subwf	EEdata_temp,W
		btfss	STATUS,	C
		retlw	0		;not valid
		movlw	B'11101000'	;< 0xE8 (0xE7 maximum)
		subwf	EEdata_temp,W
		btfsc	STATUS, C
		retlw	0		;not valid
		retlw	1

l_cvwr_1	movf	EEAddrBuf, W
		sublw	PRIM_Addr
		btfss	STATUS, Z	;is it CV1 
		retlw	1
		btfss	EEdata_temp,7	;127 maximum
		retlw	2
		retlw	0
;-------------------------------------------
; bit recognizer state machine
;
l_state_sync0	clrf	state
		bsf	DEC_STATUS,	DEC_STAT_SYNC
		movlw	PREAMBLE_CNT		; reset hbit count for sync
		movwf	B_cnt
		clrf	Err
		movlw	Addr2_b			; reset 
		movwf	FSR		
;
l_next_state	incf	state,		F	; state = sync1
;
l_clr_count	movlw	b'11111000'
		andwf	FLG,		F	; clear sample counter
		incf	FLG,		F	; count 1st sample
;
Int_end		movf	_pclath,	W
		movwf	PCLATH
		swapf	_status,	W
		movwf	STATUS
		swapf	_w,		F
		swapf	_w,		W
		retfie
;
Int_pwm		;btfsc	FLG2,	AM
		;return
		btfss	pwmmode,PWM_DCmode
		goto	int_pwm_3
		btfsc	FLG2,	AM
		return
int_pwm_3	movf	TStep,	W
		subwf	PWM_t,	W
		btfss	STATUS,	C	; it's time to count a step
		return
		movwf	PWM_t		; get residue
		incf	PWM_cnt, F
		incf	PWM_cnt, F	; count 128 steps
		btfss	pwmmode, PWM_EMF; is BEMF on?
		goto	l_ipwm_2
		movf	PWM_val, W
		subwf	EMFcutoff, W	; are we above EMF cutoff
		btfss	STATUS, C
		goto	l_ipwm_2	; yes. ignore feedback
;
		movlw	.200		; motor off period
		subwf	PWM_cnt, W
		btfss	STATUS,	 C
		goto	l_ipwm_2
		movlw	.232		; sample time
		subwf	PWM_cnt, W
		btfsc	STATUS,	 C
		btfsc	FLG2,	 SM2	
		goto	l_Int_clr_pwm	; 
		bsf	ADCON0,  2	; Start A/D conversion
		bsf	FLG2,	SM	; mark sample ready for EMF_control
		bsf	FLG2,	SM2	; prevent multiple samples
		goto	l_Int_clr_pwm
;
l_ipwm_2	bcf	FLG2,	SM2	; clear for another sample
		btfss	pwmmode, PWM_HF	; is it HF PWM
		goto	l_ipwm_1	; no. Goto LF pwm
		movf	pwmmode, W
		andlw	h'0F'
		subwf	PWM_val, W	; is the speed > transition point to HF
		btfss	STATUS,	 C
		goto	l_ipwm_1	
		movf	PWM_thr, W	; HF pwm no need to handle here
		movwf	CCPR1L
		return
;
l_ipwm_1	movf	PWM_thr, W
		subwf	PWM_cnt, W
		btfss	STATUS,		C
		goto	l_Int_set_pwm
;
l_Int_clr_pwm	clrf	CCPR1L		
		return
;
l_Int_set_pwm	movlw	h'FF'		; set to full
		movwf	CCPR1L
		return
;------------------------------------------------------------------------------------
;
l_state_sync1	btfsc	FLG,	SC_msb		; 0 or 1
		goto	l_state_sync0		; 0 hbit causes reset
		decfsz	B_cnt,		F
		goto	l_clr_count
		goto	l_next_state
;
l_state_sync2	btfsc	FLG,	SC_msb
		incf	state,		F
		goto	l_clr_count
;
l_state_sync3	btfss	FLG,	SC_msb
		goto	l_state_sync0		; 1 after 0 hbit of preamble not accepted	
		bcf	DEC_STATUS,	DEC_STAT_SYNC
		movlw	(SAMPLE_PRD * FREQ / 8) - 2
		subwf	TMR0,		F	; adjust sample time	
		movlw	h'FE'
		movwf	Buf
		goto	l_next_state
;
l_state_hbit	incf	FLG,		F	; add one count so that 3 samples are 4, over the threshold
		incf	state,		F	; do not reset the count on hbit. change state to l_state_bit.
		call	Int_pwm
		goto	Int_end	


;-------------------------------------------
;  Consist control, activation/ deactivation
;
consist_ctl	incf	B_rem,	F	; 2 byte instruction
		movlw	h'0E'
		andwf	Cmd_b,	W
		xorlw	2		; 001x == consist fwd/back
		btfss	STATUS,	Z
		goto	l_roll	
		bcf	Chk_b,	7	; original bit 7 should be '0' ('1' is reserved)
		btfss	Cmd_b,	0	; is it normal or reversed ?
		bsf	Chk_b, 	7
		movlw	Consist_addr
		movwf	EEAddrBuf
		movf	Chk_b, 	W
		call	ee_write	; inside, calls refresh_ram and updates all registers/ flags
		goto	l_roll

;------------------------------------
; access routines - eeprom
;
;
ee_read		movwf	EEAddrBuf	; address in w
;
ee_read_1	movlw	Version		; check if addr == CV7 or CV8 (read only) which are not implemented in EEPROM
		xorwf	EEAddrBuf, W
		btfsc	STATUS,	Z
		retlw	VERSION_VAL
		movlw	ManId
		xorwf	EEAddrBuf, W
		btfsc	STATUS,	Z
		retlw	MAN_ID_VAL
;
		bcf	STATUS,	RP0
		movf	EEAddrBuf, W
		bsf	STATUS, RP1	; bank 2
		movwf	EEADR
		bsf	STATUS, RP0	; bank 3
		bsf	EECON1, RD
		bcf	STATUS, RP0	; bank 2
		movf	EEDATA, W
		bcf	STATUS, RP1	; bank 0
		return

; address in Arg1, value is in W reg.
ee_write	bcf	INTCON, GIE 	;disable int
		clrwdt
		bcf	STATUS,	RP0
		bsf	STATUS, RP1	; bank 2
		movwf	EEDATA
		bcf	STATUS, RP1
		movf	EEAddrBuf, W
		bsf	STATUS, RP1
		movwf	EEADR
		bsf	STATUS, RP0
		bsf	EECON1, WREN 	;enable write
		bcf	EECON1, EEPGD
		movlw	h'55'
		movwf	EECON2		; write 55
		movlw	h'AA'
		movwf	EECON2
		bsf	EECON1, WR	; set wr bit, begin wr
; make sure write completed & verified
ee_wr_wait	btfsc	EECON1, WR
		goto	ee_wr_wait
		bcf	EECON1,	WREN
;
ee_wr_fin	bcf	STATUS, RP1
		bcf	STATUS, RP0	; bank 0
		bsf	INTCON, GIE	;enable int
		goto	refresh_ram	; put all time-critical values in ram copies



;------------------------------------
; read saved CVs from eeprom
; Arg5 and Arg3 are changed
;
read_eeprom_file 	movlw	1
		movwf	PWM_timer
		movlw	b'11000000'
		movwf	DEC_STATUS
;
refresh_ram	movlw	PWMTot		; (CV9 << 2) ^ 0x83
		call	ee_read
		movwf	Arg5
		rlf	Arg5,	F
		rlf	Arg5,	F
		movlw	0x83
		iorwf	Arg5,	W
		movwf	TStep
;
		movlw	PWM_Mode
		call	ee_read
		movwf	pwmmode
;
		movlw	EMFCut		; read EMF constants
		call	ee_read
		movwf	EMFcutoff
		rlf	EMFcutoff, F	; normalize to 0-254
		movlw	Ki
		call 	ee_read
		movwf	EMF_Ki
		movlw	Kp
		call	ee_read
		movwf	EMF_Kp
		movlw	Kfr
		call	ee_read
		movwf	EMF_Kfr
;
		movlw	Config_data_1	; read config byte (CV29)
		call	ee_read
		movwf	DEC_config	; move to RAM copy
		btfss	DEC_config, DEC_CFG_AA	; is adv. addr on?
		goto	read_ef_1
		movlw	ExtAddr1
		call	ee_read
		movwf	Decoder_addr
		movwf	Active_addr
		movlw	ExtAddr2
		call	ee_read
		movwf	Decoder_addr_x
		movwf	Active_addr_x
		goto	read_ef_2	
;
read_ef_1	clrf	Decoder_addr_x
		clrf	Active_addr_x
		movlw	PRIM_Addr	; read primary address
		call 	ee_read
		movwf	Decoder_addr
		movwf	Active_addr
;
read_ef_2	bcf	DEC_STATUS,	DEC_STAT_CO
		movlw	Consist_addr	; read consist address
		call 	ee_read
		movwf	Arg5
		bcf	DEC_STATUS,	DEC_STAT_CR
		btfsc	Arg5,	7
		bsf	DEC_STATUS,	DEC_STAT_CR
		andlw	b'01111111'	; is consist addr == 0 (inactive)
		btfsc	STATUS,	Z
		return
		movwf	Active_addr	; activate consist
		clrf	Active_addr_x	; ext. byte is 0
		bsf	DEC_STATUS, 	DEC_STAT_CO
		return
;---------------------------------------
; write contents of DEC_config into CV29
;
write_config	movlw	Config_data_1	; CV29
		movwf	EEAddrBuf
		movf	DEC_config,	W
		call	ee_write
		return
;------------------------------------
; Multiply
; M1 = W; M2 = Arg1; Prod = Arg2:Arg1
; Arg3 is cleared
;
multiply		movwf	Arg2
		movlw	d'8'
		movwf	Arg3		; counter
		movf	Arg2, 	W
		clrf	Arg2
;
l_mult_loop	rrf	Arg1,	F
		btfsc	STATUS, C
		addwf	Arg2, 	F
		rrf	Arg2,	F
		decfsz	Arg3, 	F
		goto 	l_mult_loop
		rrf	Arg1,	F	; one additional rotate
		return
;------------------------------------
; spline calculation
; PWM_step = step256 [4 : 255]
; V(W) = {X1}  [W * (W - 128) * V_High] / (128 * 256) -            
;	 {X2}  [(W - 128)(256 - W)* V_Start] / (128 * 256) +
;	 {X3}  [W * (256 - W) * V_Mid ] / (64  * 256)
;
splineIt	movf	PWM_step, W
		btfss	STATUS, Z
		goto	l_spl_nz
		clrf	PWM_val
		return
;
l_spl_nz	btfss	DEC_config,	DEC_CFG_ST
		goto	l_spl_start	; calculate curve
		movwf	Arg1
		movlw	d'27'		; get 28ss
		call	multiply	; Arg2 is step28, Arg1 is fraction
		movlw	SPD_Tbl + 1	; get T[step28 +1]
		addwf	Arg2,	F
		movf	Arg2,	W
		call	ee_read		; read speed from table
		movwf	Arg4
		decf	Arg2,	W
		call	ee_read
		movwf	Arg5
		subwf	Arg4,	W	; W == T[step28 + 1] - T[step28]
		call	multiply	; Arg2 = interpolation value
		movf	Arg2,	W
		addwf	Arg5,   W
;
l_spl_end	movwf	PWM_val
		return	
;
l_spl_start	clrf	Arg6
		movlw	V_High
		call	ee_read
		movwf	Vmax
		andlw	h'FE'
		btfsc	STATUS, Z	; test for default case
		movlw	h'FF'
		movwf	Arg1
		movwf	Vmax
		bcf	STATUS, C
		rrf	Arg1,	F	; convert to 7 bit numbers
		movlw	V_Start
		call	ee_read
		movwf	Vmin
		movwf	Arg4		; save it for later
		bcf	STATUS,	C
		rrf	Arg4,	F
		movf	Arg4,	W
		addwf	Arg1,	F	; guaranteed no overflow
		movf	PWM_step, W	; PWM_step is actually [W + 1] and curve starts at 1
		call	multiply	; now Arg2== E1== W*(V_High + V_Start) / 256
		movf	Arg4,	W
		subwf	Arg2,	W
		btfsc	STATUS,	C
		goto	l_spl_c1
		incf	Arg6,	F
		sublw	0
;
l_spl_c1	movwf	Arg1		; get ready to multiply by (W - 128)/128
		movlw	d'128'		
		subwf	PWM_step, W
		btfsc	STATUS, C	; is result negative?
		goto	l_spl_c3	; no.
		sublw	0		; negate
		incf	Arg6,	F	; reverse sign
		bsf	Arg6,	7	; remember graph area
;
l_spl_c3	call	multiply
		rlf	Arg1,	F	; to increase accuracy do not lose bits early	
		rlf	Arg2,	W	; now w == E1 * |W - 128| / 128
		btfsc	Arg6,	0	; is result so far negative?
		sublw	0		; negate, so we get the real comp' 2 number
		movwf	Arg5
;
		movf	PWM_step, W	; PWM_step is [W + 1]
		movwf	Arg1
		sublw	0
		call	multiply
		movlw	V_Mid		; calc E3 == (W * (256-W) * V_Mid) / (64 * 256)
		call	ee_read
		andlw	h'FE'
		btfss	STATUS,	Z	; default case
		goto	l_spl_c4
		movf	Vmin,	W	;set mid value
		addwf	Vmax,	F
		rrf	Vmax,	W	;halve it

l_spl_c4	movwf	Arg1
		bcf	STATUS,	C
		rrf	Arg1,	F
		movf	Arg2,	W
		call	multiply
		rlf	Arg1,	F
		rlf	Arg2,	F
		rlf	Arg1,	F
		rlf	Arg2,	W
		addwf	Arg5, 	F	; that's it!
		btfsc	Arg5, 	7	; do not want residue of overflow, so max out 
		goto	l_spl_ovf	
		rlf	Arg5, 	W
		addlw	1
		goto	l_spl_end
;
l_spl_ovf	movlw	0	; if underflow in left half of curve, fix to 0
		btfss	Arg6,	7	
		movlw	h'FF'	; if overflow in right half of curve, fix to 255
		goto	l_spl_end

;------------------------------------
;  EMF feedback control
;
;  PWM_val  - desired value       (C[n])
;  ADRESH  - current EMF value   (F[n])
;  PWM_thr  - PWM control signal  (R[n])
;
;  Implemented a PDFF (PI+) controller
; R(z) = (Ki * Z) / (Z - 1)*[C(z) - F(z)]  + KP * [Kfr * C(z) - F(z)]
; R[n] = R[n-1] + Ki(C[n] - F[n]) + Kp * [Kfr * (C[n] - C[n-1]) - (F[n] - F[n-1])]
; if the recursive expression is expanded into a series, assuming initial constraints 0, the result is:
; R[n] = SUM(j=1,n)(Ki*(C[j] - F[j])) + Kp* (Kfr * C[n] - F[n])



EMF_control	movf	PWM_val, W
		btfss	STATUS,	 Z	; make sure a stop command is not calculated.
		btfss	pwmmode, PWM_EMF; is EMF mode on?
		goto	EMF_c_end	
		subwf	EMFcutoff, W	; are we above EMF cutoff speed
		movf	PWM_val, W
		btfss	STATUS,	C
		goto	EMF_c_end
		btfss	FLG2,	SM	; is there a sample ready?
		return
		btfsc	ADCON0, 2	; conversion complete?
		return
;Integral
		clrf	Arg6		; sign buffer
		subwf	ADRESH,	W	; W = -(C[n] - F[n])
		btfsc	STATUS,	C	; if cleared error term is positive
		goto	EMF_c_8
		bsf	Arg6,	0	; mark positive error term
		sublw	0		; change sign for multiplication

EMF_c_8		movwf	Arg1		
		incf	Arg1,	F	; fix rounding errors (Ki is 255/256 ?)
		movf	EMF_Ki,	W
		call	multiply
		movf	PWM_val,W
		sublw	.15
		movf	Arg2,	W
		btfss	STATUS,	C
		goto	EMF_c_12
		movlw	h'F0'
		andwf	Arg2,	W	; is it going to overflow
		btfss	STATUS,	Z
		goto	EMF_c_11
		movlw	h'F0'		; the following code is mult. by 16
		andwf	Arg1,	F
		swapf	Arg1,	F
		swapf	Arg2,	F
		andwf	Arg2,	W
		iorwf	Arg1,	W
		goto	EMF_c_12
		
EMF_c_11	movlw	h'FF'
		
EMF_c_12	btfsc	Arg6,	0	; set if error term is positive
		goto	EMF_c_9
		subwf	Integ,	W
		btfss	STATUS, C
		clrw
		goto	EMF_c_10

EMF_c_9		addwf	Integ, 	W
		btfsc	STATUS, C
		movlw	h'FF'

EMF_c_10	movwf	Integ

;filtered proportional

		clrf	Arg6
		movf	PWM_val, W
		movwf	Arg1
		movwf	Arg2		; in case we have to skip the multiplication
		movf	EMF_Kfr, W	; gradually fading Kfr effect (increase from EMF_Kfr to h'ff') with demand speed
		subwf	Arg2,	F
		btfsc	STATUS, C	; if positive then we are under the threshold speed
		movf	PWM_val, W
		call	multiply
;
EMF_c_7		movf	ADRESH,	W
		subwf	Arg2,	W	;W = Kfr*I[n] - F[n]
		btfsc	STATUS,	C	;is it negative
		goto	EMF_c_3
		bsf	Arg6,	0
		sublw	0		; save sign and negate for mult. by Kp
;
EMF_c_3		movwf	Arg1
		movf	EMF_Kp,	W
		call	multiply
		movlw	h'F0'
		andwf	Arg2,	W	; is it going to overflow
		btfss	STATUS,	Z
		goto	EMF_c_4
		movlw	h'F0'		; the following code is mult. by 16
		andwf	Arg1,	F
		swapf	Arg1,	F
		swapf	Arg2,	F
		andwf	Arg2,	W
		iorwf	Arg1,	W
		goto	EMF_c_5
;
EMF_c_4		movlw	h'FF'
;
EMF_c_5		btfsc	Arg6,	0
		goto	EMF_c_6
		addwf	Integ,	W
		btfsc	STATUS,	C
		movlw	h'FF'		;overflow
		goto	EMF_c_end
;
EMF_c_6		subwf	Integ,	W
		btfss	STATUS,	C	;underflow
		movlw	0
;
EMF_c_end	movwf	PWM_thr		; now has the new control value
		bcf	FLG2,   SM	; can get new sample now
		return


	
;------------------------------------
; start point

hard_reset	movlw	Consist_addr		; zero in hard-reset
		movwf	EEAddrBuf
		clrw
		call	ee_write
		movlw	B'00000010'		; default CV29
		movwf	DEC_config
		call	write_config
		clrw
		incf	EEAddrBuf,	f		; Clear error CV 
		call	ee_write
		movlw	PRIM_Addr
		movwf	EEAddrBuf
		movlw	3			; default short address 
		call	ee_write
;
init_start	bsf	STATUS,RP0	
		bsf	STATUS,RP1		;Bank 3
		bcf	EECON1,EEPGD
		bcf	STATUS,RP1		;Bank 1
		movf	PCON,	W
		
		bcf	STATUS,RP0
		bcf	INTCON,	GIE
		movwf	Arg4  
		
		
	
		clrf	Arg6
	
l_start_lp	btfsc	pwmmode,PWM_DCmode
		goto	l_start_c4
		btfsc	IN_port, IN_bit2
		goto	l_start_c4
		btfss	IN_port, IN_bit
		goto	l_start_lp		; both inputs are 0, no direction indication (PWM DC)
		bsf	Arg6, DIRF		; set Arg6 to initial direction according to polarity
		
l_start_c4	btfss	Arg4, NOT_POR		; Is it POR?
		goto	l_start_POR
		btfsc	Arg4, NOT_BOR		; is it BOR? -if not do a full reset.
		goto	start			; some other reset. MCLR?
		movlw	h'A5'			; BOR. In addition check memory valid
		xorwf	magic,	W
		btfsc	STATUS, Z
		goto	l_start_pulse
		xorwf	magic,	F
;
l_start_POR	clrf	FLG2			; DG is only cleared upon a real POR
		clrf	Fn_control
		bsf	Fn_control,	FN_RST	; signal 1st speed command is next, for momentary reset
		clrf	PWM_step		; initially stop.
		btfsc	pwmmode,PWM_DCmode
		goto	l_start_P1
		movf	Arg6,	W
		iorwf	FLG2,	F		; set sensed direction into FLG2[DIRF]
l_start_P1	movlw	b'10000000'
		movwf	FLG
;
start		bcf	INTCON, GIE		; temp. disable timer int
		movlw	b'00111111'
		movwf	ON_Effect
		clrf	PWM_step_tgt
		movlw	DIRF_msk | DG_msk
		andwf	FLG2,	F
		call	read_eeprom_file
		movlw	PRIM_Addr
		call	ee_read
		btfsc	STATUS, Z		; is primary address 0?
		goto	l_analog_mode		; yes. analog mode	
		btfsc	STATUS, NOT_TO		; skip if a WDT reset
		goto	l_digital_mode

l_analog_mode	clrwdt
		btfss	DEC_config, DEC_CFG_PW 		; get analog mode enable
		goto	l_digital_mode
		bsf	FLG2,	AM
		btfsc	pwmmode,PWM_DCmode
		goto	l_start_pulse
		bcf	Fn_control,	FN_RST	; no need for this if coming from analog to DCC
		movf	FLG2,	W		; are directions equal?
		andlw	DIRF_msk
		xorwf	Arg6,	W		; W = FLG2[DIRF] xor Arg6
		btfss	FLG2,	DG
		goto	l_start_c2
		btfsc	STATUS, Z		; skip if no
		goto	l_start_c3		; set PWM = h'FF'
		goto	l_start_pulse
l_start_c2	xorwf	FLG2,	F		; set direction to Arg7!
l_start_c3	comf	PWM_step_tgt, F		; This would set target speed to full scale if direction matches
		goto	l_start_pulse
;
l_digital_mode  clrf	PWM_val
		clrf	PWM_thr
		clrf	Integ
		clrf	PWM_cnt
		clrf	PWM_Accel_cnt
		clrf	PORTA
		clrf	PORTB
		clrf	PORTC
		clrf	Sec_cnt
;
l_start_pulse	bsf	STATUS, RP0		; bank 1
		bsf	PCON,	0		; have to manually set PCON bits
		bsf	PCON,	1
		call	set_power
		call	pwm_setd_start		; cause the actual direction to be set
		call 	splineIt		; set initial value for PWM_val
;
p_sync_start	clrf	state		; reset
;
p_delay_start	clrf	B_cnt
;
p_delay		movlw	Addr2_b
		movwf	FSR		; reset pointer
		clrf	Err
		clrf	Sup_1
		clrf	Sup_2
		bcf	FLG,	VP	; release packet buffers
		bsf	INTCON, T0IE
		bcf	INTCON, T0IF
		bsf	INTCON, GIE
;----------------------------------------
;  packet handler
; 
packet		call	pwm
		btfsc	FLG, VP		; if no valid packet, check time-out & return or sleep
		goto	l_dispatch
		movlw	Packet_TO	; read time-out value (sec)
		call	ee_read
		btfsc	STATUS,	Z	; is T/O active?
		goto	packet
		subwf	Sec_cnt, W
		btfss	STATUS, C
		goto	packet
		goto	l_spd_emr_stop




;---------------------------------------------
; update effects
;
effect_update	incf	Effect_cnt,	F	; rolls ~every 1sec
		btfsc	STATUS,		Z
		incf	Sec_cnt,	F
		movlw	b'00011000'
		andwf	ON_Effect,	F
		movlw	0x40
		andwf	Effect_cnt,	W
		iorlw	0x83
		iorwf	ON_Effect,	F
		bcf	ON_Effect,	EF_QB
		btfss	ON_Effect,	EF_QA
		bsf	ON_Effect,	EF_QB
		call	MARs
		andwf	Effect_cnt,	W
		btfss	STATUS,		Z
		bsf	ON_Effect,	EF_MARs
		movlw	0x06
		andwf	Effect_cnt,	W
		btfsc	STATUS,		Z
		bcf	ON_Effect,	EF_Dim
		movlw	0x78
		andwf	Effect_cnt,	W
		btfsc	STATUS,		Z
		bcf	ON_Effect,	EF_ST	
		decfsz	Chaf_cnt,	F
		goto	l_ef_func
		bcf	ON_Effect,	EF_SPD	
		rrf	PWM_thr,	W
		sublw	CHAF_PRESET_CNT
		movwf	Chaf_cnt
	
;
; handle functions
;
l_ef_func	clrf	Arg5			; result output mask
		movlw	FN_NUM
		movwf	Arg6			; Fn CV ( reversed ) index
		movlw	1
		movwf	Arg3			; used as bit mask
;
l_ef_loop	movf	Arg3,		W
		andwf	Fn_control,	W
		btfsc	STATUS,		Z
		goto	l_ef_c1
		movf	Arg6,		W
		sublw	FL_Effect + FN_NUM
		call	ee_read
		andwf	ON_Effect,	W 	; if 0, turn on function outputs 
		btfss	STATUS,		Z
		goto	l_ef_c1
		movf	Arg6,		W
		sublw	FL_loc + FN_NUM
		call	ee_read
		iorwf	Arg5,		F
;
l_ef_c1		bcf	STATUS,		C
		rlf	Arg3,		F
		decfsz	Arg6,		F
		goto	l_ef_loop
;  
;  when we finish this loop, Arg5 has the bits set for the output lines on/of status
;
		movlw	Out_Active_Low
		xorwf	Arg5,		F	; set correct active polarity
;
		btfss	Arg5,		OUT1		; bit 0 is logical out1
		bcf	OUT1_port,	OUT1_pad
		btfsc	Arg5,		OUT1
		bsf	OUT1_port,	OUT1_pad
		btfss	Arg5,		OUT2		; bit 1 is logical out2
		bcf	OUT2_port,	OUT2_pad
		btfsc	Arg5,		OUT2
		bsf	OUT2_port,	OUT2_pad
		btfss	Arg5,		OUT3		; bit 3 is logical out3
		bcf	OUT3_port,	OUT3_pad
		btfsc	Arg5,		OUT3
		bsf	OUT3_port,	OUT3_pad
		btfss	Arg5,		OUT4		; bit 4 is logical out4
		bcf	OUT4_port,	OUT4_pad
		btfsc	Arg5,		OUT4
		bsf	OUT4_port,	OUT4_pad
		btfss	Arg5,		OUT5		; bit 5 is logical out5
		bcf	OUT5_port,	OUT5_pad
		btfsc	Arg5,		OUT5
		bsf	OUT5_port,	OUT5_pad
;
		return

;---------------------------------------------
; Final direction change following a slow-down
;
pwm_set_dir	btfss	FLG2,	DIRC
		return
pwm_setd_start	bcf	FLG2,	DIRC	; unmark the slow-down (*)

		btfsc	FLG2,	DIRF
		goto	pwm_setd_fwd
;
		bcf	OUT_fwd_port, 	OUT_fwd
		bsf	ON_Effect, 	EF_Fwd
		bsf	OUT_back_port,	OUT_back
		bcf	ON_Effect, 	EF_Rev
		goto	pwm_setd_spd

pwm_setd_fwd	bcf	OUT_back_port, 	OUT_back
		bsf	ON_Effect, 	EF_Rev
		bsf	OUT_fwd_port,	OUT_fwd
		bcf	ON_Effect, 	EF_Fwd
;
pwm_setd_spd	movf	PWM_step_tgt,	W
		movwf	PWM_step_tg1
		return
		

;---------------------------------------------
; PWM routine - handles acceleration / deceleration
;
;
pwm		btfss	FLG2,	AM		; next test for analog mode
		goto	l_pwm_3
		btfss	FLG2,	POL		; make sure there is exactly one WDT following a direction change.
		clrwdt				; if the flag is clr prevent WDT
	
l_pwm_3		movlw	PWM_CNT_PRESET		; compare to preset cycle value (for 256 increments)
		subwf	PWM_Accel_cnt, 	W
		btfss	STATUS,		C
		goto	l_pwm_4			; no accel/decel
		movwf	PWM_Accel_cnt		; get the residue
;
;	effects are handled here
		call	effect_update
	
;
		decfsz	SVC_timer,	F	; is svc mode expired
		goto	l_pwm_2
		bcf	DEC_STATUS, 	DEC_STAT_SV ; clear svc mode
;
l_pwm_2		decfsz	PWM_timer,	F	; count down based on CV#3 + CV#23 or CV#4 + CV#24
		goto	l_pwm_4
		movf	PWM_step, 	W
		subwf	PWM_step_tg1, 	W
		btfsc	STATUS,		Z	; if equal no acceleration needed
		goto	l_pwm_5

		btfss	STATUS, 	C	; if positive, accel needed; otherwise decel
		goto	l_pwm_decel	
		incf	PWM_step, 	F	; increment now
		movlw	ACC_Rate
		call	ee_read
		movwf	PWM_timer		; preset to CV
		movlw	Accel_Adj		; add adjustment
		goto	l_pwm_1
;
l_pwm_decel	decf	PWM_step, 	F	; decelerate
		movlw	DEC_Rate
		call	ee_read
		movwf	PWM_timer
		movlw	Decel_Adj		; add adjustment
;
l_pwm_1		call	ee_read
		movwf	Arg1
		andlw	h'7F'
		btfsc	Arg1, 		7	; add or subtract?
		sublw	0			; W := 0 - W
		addwf	PWM_timer, 	F
		btfss	STATUS,		Z
		goto	l_pwm_7
		movf	PWM_step_tg1,	W
		movwf	PWM_step
;
l_pwm_7		call	splineIt
;
l_pwm_5		incf	PWM_timer,	F
		movf	PWM_step,	F
		btfsc	STATUS,		Z	; if stopped, adjust direction
		call	pwm_set_dir
;
l_pwm_4		call	EMF_control
		retlw	0

;----------------------------------------
;  byte constructor and error check
;
byte		rlf	FLG, 		W		; move LV -> C
		btfss	FLG, 		VB		; skip if byte complete
		goto	l_rotate
		bcf	FLG, 		VB
		btfss	STATUS, 	C		; look for end-of-packet bit
		goto	l_rotate
		bsf	DEC_STATUS,	DEC_STAT_BP	; mark beginning of packet
		movf	Err,		F
		btfss	STATUS,		Z
		retlw	0				; error
		movlw	4				; verify min. packet length (treat as ext. addr)
		subwf	B_cnt,		F
		btfss	STATUS,		C
		retlw	0
;
		incf	B_cnt,		F		; B_cnt - 4 + 1, subtract addr bytes and err byte from length
		bsf	FLG,		VP		; packet is ok
;
l_rotate	rlf	Buf,		F	
		btfsc	STATUS, 	C 		; skip if bit == 0, start bit (end of byte)
		retlw	1				; not a byte 
		bsf	FLG,		VB
		btfsc	FLG,		VP
		retlw	0				; if packet marked, drop byte on the floor and error
		movf	Buf,		W
		xorwf	Err,		F
		btfss	DEC_STATUS,	DEC_STAT_BP
		goto	l_byte_3
		bcf	DEC_STATUS,	DEC_STAT_BP
		andlw	b'11000000'			; test if high address bits are '11', extended address
		xorlw	b'11000000'
		btfsc	STATUS,		Z		; if it is, start at Addr2_b
		goto	l_byte_2
		clrf	Addr2_b		; ensures a comparison of short addr as ext. Addr will succeed (MSB is 0)
		incf	FSR,		F		; if not, skip the MSB of address, point to Addr_b
		incf	B_cnt,		F		;
;
l_byte_2	movf	Buf,		W; put original addr back
;
l_byte_3	movwf	INDF
		incf	FSR,		F
		incf	B_cnt,		F
		movlw	7				; pkt size < 7 bytes
		subwf	B_cnt,		W
		btfsc	STATUS,		C
		retlw	0				; error
;
l_new_byte	clrf	Buf
		comf	Buf,		F
		retlw	1



;----------------------------------------------------
; Is this a repeat packet
;  required for Service mode & CV access instructions
;
isRepeatPacket	bsf	DEC_STATUS,	DEC_STAT_DP	
		movf	Addr2_b,	W
		xorwf	LastAddr2_b,	W
		btfss	STATUS,		Z
		bcf	DEC_STATUS,	DEC_STAT_DP		
		xorwf	LastAddr2_b,	F
		movf	Addr_b,		W
		xorwf	LastAddr_b,	W	; should be zero
		btfss	STATUS,		Z
		bcf	DEC_STATUS,	DEC_STAT_DP		
		xorwf	LastAddr_b,	F
		movf	Cmd_b,		W
		xorwf	LastCmd_b,	W
		btfss	STATUS,	Z
		bcf	DEC_STATUS,	DEC_STAT_DP		
		xorwf	LastCmd_b,	F
		movf	Chk_b,		W
		xorwf	LastChk_b,	W
		btfss	STATUS,	Z
		bcf	DEC_STATUS,	DEC_STAT_DP		
		xorwf	LastChk_b,	F
		movf	Sup_1,		W
		xorwf	LastSup1,	W
		btfss	STATUS,	Z
		bcf	DEC_STATUS,	DEC_STAT_DP		
		xorwf	LastSup1,	F	
		movf	Sup_2,		W
		xorwf	LastSup2,	W
		btfss	STATUS,	Z
		bcf	DEC_STATUS,	DEC_STAT_DP		
		xorwf	LastSup2,	F	
		return


;--------------------------------------
; set advanced acknowledgement
;
set_adv_ack
;		bsf	DEC_config, DEC_CFG_AK
;		goto	l_clr_ak_1

;--------------------------------------
; clr advanced acknowledgement
;
clr_adv_ack
;		bcf	DEC_config, DEC_CFG_AK
l_clr_ak_1	call	write_config
		goto	ack
;--------------------------------------
; set adv. addressing
;
set_adv_addr	bsf	DEC_config, DEC_CFG_AA
		goto	l_clr_ak_1
;--------------------------------------
; clr adv. addressing
;
clr_adv_addr	bcf	DEC_config, DEC_CFG_AA
l_clr_aa_1	goto	l_clr_ak_1
;--------------------------------------
;  acknowledge request response. 
;  Note: this is implemented as a 5ms 
;  motor-on, basic ack
;
ack		btfss	DEC_STATUS, DEC_STAT_SV	; if not in svc mode, do not ack
		goto	l_roll
		bcf	INTCON,	GIE		; clear interrupts
		movlw	FREQ * 2
		movwf	Arg3			; count 17 * 256 * 3 ~= 6.5 milisec
		;movlw	OUT_back_msk		; reverse direction with every ack
		;xorwf	OUT_back_port,	F	; this didn't work for some reason	
		;movlw	OUT_fwd_msk
		;xorwf	OUT_fwd_port,	F
		bcf	OUT_back_port,	OUT_back	; old code
		bsf	OUT_fwd_port,	OUT_fwd
		bsf	OUT1_port, 	OUT1_pad	; turn on FL
		bsf	OUT2_port, 	OUT2_pad	; turn on RL
		movlw	h'FF'
		movwf	CCPR1L			; turn on motor for >= 5msec
;
l_ack_delay	clrwdt
		clrf	Arg4
;
l_ack_loop	decfsz	Arg4,		F
		goto	l_ack_loop	
		decfsz	Arg3, 		F
		goto	l_ack_delay
		clrf	CCPR1L
		bcf	OUT1_port, 	OUT1_pad	; FL, RL off
		bcf	OUT2_port, 	OUT2_pad
		goto	p_sync_start		; no need to roll in svc mode

;----------------------------------------
; roll packet bytes
;	W : number of bytes
l_roll		movf	B_rem,	W
		subwf	B_cnt,	F		; decrease byte count
		btfsc	STATUS,	Z
		goto	p_delay			; continue
		btfss	STATUS, C	
		goto	p_sync_start		; error, reset
;
l_roll_1	movf	Chk_b,	W
		movwf	Cmd_b
		movf	Sup_1,	W
		movwf	Chk_b
		movf	Sup_2,	W
		movwf	Sup_1
		decfsz	B_rem,	F
		goto 	l_roll_1
		goto	l_parse

		
;----------------------------------------
; Set Direction 
; Z-bit - direction
set_dir		bsf	FLG2,		DG
		bcf	FLG2,		DIRC		; def. is direction not changed
		btfss	DEC_STATUS,	DEC_STAT_AC	; is this a consist command?
		goto	l_setd_1
		btfsc	DEC_STATUS,	DEC_STAT_CR
		xorwf	Arg1,		W
		goto	l_setd_2
;
l_setd_1	btfsc	DEC_config,	DEC_CFG_DR	; is direction reversed
		xorwf	Arg1,		W		; reverse bit
;
l_setd_2	andwf	Arg1,		W
		btfss	STATUS,	Z
		goto	l_setd_fwd
;							; backwards direction
		bcf	FLG2,		DIRF		; signal back direction
		btfsc	ON_Effect,	EF_Rev
		goto	l_setd_3			; (*)
		return
;
l_setd_fwd	bsf	FLG2,		DIRF		; signal fwd direction
		btfss	ON_Effect,	EF_Fwd		; (*)
		
		return

l_setd_3	bsf	FLG2,		DIRC		; direction has changed (*)
		btfsc	Fn_control,	FN_RST		; (*)
		call	pwm_setd_start			; (*)
		return
;----------------------------------------
; Set   lamp fl/rl
;
set_lmp		movlw	CAct_LMP		; read mask of lights that are controlled by consist addr.
		call	ee_read
		btfss	DEC_STATUS, 	DEC_STAT_AC	; if not consist addr, then both lights are controlled here
		movlw	b'00000011'			; both lamp Fn's
		xorlw	h'FF'
		andwf	Fn_control,	F		; first, mask controlled lights off
		xorlw	h'FF'
		btfss	Cmd_b,		4		; this is the FL bit
		clrw
		iorwf	Fn_control,	F	
		bcf	Cmd_b, 		4	 	; for speed/dir: set bit for intermediate step to 0
		return				
	
;----------------------------------------
; set power mode (DCC/Analog)
;			
;					;On entry assumes Bank 1
set_power	movlw	b'11001001'
		movwf	OPTION_REG
		movlw	B'00001110'	;set adcon1 a0 analog
		movwf	ADCON1
		movlw 	B'11111111'	;set PR2 to 255
		movwf	PR2
		bsf	STATUS,RP1	; bank 3
		clrf	EECON1		;ensure no program writes
		bcf	STATUS,RP1
 		bcf	STATUS,RP0	; bank 0
		movlw	B'00001100'
		movwf	CCP1CON		;set to pwm mode
		movlw	B'00000100'	;timer 2 on, prescale = 1
		movwf	T2CON
		movlw	B'10000001'	;adc control
		movwf	ADCON0
	        clrf    INTCON
;	        clrf	CCPR1L		;PWM width = 0
		btfsc	FLG2,		AM
		goto	l_sep_analog
;
		bsf	STATUS, RP0		; bank 1
		movlw	PORTA_Input_msk
		movwf	TRISA
		movlw	PORTB_Input_msk		; must keep RB0 as input (marklin change direction)
		movwf	TRISB
		movlw	PORTC_Input_msk
		movwf	TRISC
		bcf	STATUS, RP0
 		clrf    PORTA
		clrf    PORTB
		clrf	PORTC
		return
;
l_sep_analog	bsf	STATUS, RP0		; bank 1
		movlw	PORTA_In_Analog_msk	; keeps lights output
		movwf	TRISA
		movlw	PORTB_In_Analog_msk	; keeps lmp, pwm bit output	
		movwf	TRISB
		movlw	PORTC_In_Analog_msk
		btfss	pwmmode,PWM_DCmode
		goto	l_sep_an1

		movlw	B'11111111'
l_sep_an1	movwf	TRISC
		bcf	STATUS,	RP0		; bank 0
		movlw	PORTB_ON_Analog_msk	; fixed mask for outputs that are on in analog mode
		movwf	PORTB
		movlw	PORTA_ON_Analog_msk
		movwf	PORTA
		movlw	PORTC_ON_Analog_msk
		movwf	PORTC
		return

;----------------------------------------
; Advanced op
;
adv_op		movf	Cmd_b,	w
		xorlw	b'00111111'	; 128 speed step
		btfss	STATUS,	Z
		goto	l_roll
		incf	B_rem,	F	; 2 byte instruction
		movlw	b'10000000'	; direction bit mask for 128ss
		movwf	Arg1
		movf	Chk_b,	W
		call	set_dir
		rlf	Chk_b,	W
		andlw	h'FE'		; (0 - 127) -> (0 - 254)
		btfsc	STATUS,	Z	; is it stop (0)?
		goto	l_spd_stop
		addlw	-2		; (2 - 254) -> (0 - 252)
		btfsc	STATUS,	Z	; emergency stop?
		goto	l_spd_emr_stop
		addlw	-2		; adjust (2 - 252) to (0 - 250)
		goto	l_spd_calc

;----------------------------------------
; Speed and direction
;
speed_dir	bcf	DEC_STATUS,	DEC_STAT_NG
		movlw	b'00100000'	; direction bit mask for 14/28 ss
		movwf	Arg1
		movf	Cmd_b,	W
		call	set_dir
		btfss	DEC_config,	DEC_CFG_FL
		call	set_lmp
;
l_speed		movlw	b'00011111'
		andwf	Cmd_b, 	F
		movf	Cmd_b,	W
		andlw	b'00001111'
		btfsc	STATUS,	Z
		goto	l_spd_stop
		addlw	-1		
		btfsc	STATUS,	Z
		goto	l_spd_emr_stop
		bcf	STATUS,	C
		rlf	Cmd_b,	F	; make 28 spd steps out of 16
		btfsc	Cmd_b,	5	; bit 4 controls intermediate step
		bsf	Cmd_b,	0	; 
		bcf	Cmd_b,	5	; erase bit 4
		movf	Last_spd, W
		subwf	Cmd_b,	W
		btfsc	STATUS,	C	; treat negative values
		goto	l_spd_delta
		bsf	DEC_STATUS, DEC_STAT_NG
		addlw	2
;
l_spd_delta	addlw	-1		; at this point, if |Cmd_b - Last_spd| == 1, then W == 0
		movwf	Arg6		; store it for later
		movf	Cmd_b,	W	
		movwf	Last_spd	; keep last speed for half-steps
		addlw	-4		; convert (4-31) -> (0-27)
		movwf	Arg1
		movlw	STEP_MULTIPLIER ; (step-4) * 256 / 27 == (step-4) * 151 / 16
		call	multiply
		swapf	Arg2,	F	; divide by 16
		swapf	Arg1,	W
		xorwf	Arg2,	W
		andlw	h'0F'
		xorwf	Arg2,	W
		movf	Arg6,	F	; test if delta speed is 1
		btfss	STATUS,	Z
		goto	l_spd_calc

; add or subtract 5 from step256 ~= (256/27) / 2 

		btfss	DEC_STATUS, DEC_STAT_NG ; if delta is positive, subtract half step and vice versa
		addlw	-d'10'
		addlw	d'5'
;
l_spd_calc	btfsc	FLG2,	AG		; if digital following analog mode and direction changed ignore this packet.
		btfss	FLG2,	DIRC		
		goto	l_spd_c1
		bcf	FLG2,	DIRC
		movlw	DIRF_msk
		xorwf	FLG2,	F		; change direction back,
		goto	l_spd_stop1		; and slow down to stop.

l_spd_c1	addlw	1			; speed sent to splineIt is not '0'
		movwf	PWM_step_tgt
		btfsc	Fn_control,	FN_RST	; this signals 1st speed packet after reset
		movwf	PWM_step		; if it is 1st speed packet, set immediately
		bcf	Fn_control,	FN_RST
		goto	l_spd_end
;
l_spd_emr_stop	clrf	PWM_step
;
l_spd_stop	bcf	FLG2,	AG
l_spd_stop1	clrf	PWM_step_tgt
;		
l_spd_end	movf	PWM_step_tgt, W
		btfsc	FLG2,	DIRC
		clrw
		movwf	PWM_step_tg1
		call	splineIt
		call	EMF_control
		goto	l_roll

;----------------------------------------
; enhanced special functions - group 1
;	100|FL|F4|F3|F2|F1
;  currently implemented FL, F1, F2
; 
func_1		btfsc	DEC_config, 	DEC_CFG_FL	; if bit is set, control FL here
		call	set_lmp
;
		movlw	b'11000011'			; complement the mask
		andwf	Fn_control,	F
		movlw	CAct_F1_F8			; read mask for Fn controlled by consist
		call	ee_read
		btfss	DEC_STATUS, DEC_STAT_AC		; if packet not to consist addr, mask is h'FF'
		movlw	h'FF'	
		andwf	Cmd_b,		F
		rlf	Cmd_b,		F
		rlf	Cmd_b,		W
		andlw	b'00111100'
		iorwf	Fn_control,	F
;----------------------------------------
; enhanced special functions - group 1
;	1011|F8|F7|F6|F5
;  currently implemented none
; 

func_2		goto	l_roll


;----------------------------------------
; translate CV# (0 based) to EEPROM address
; CV# bits 0-7 in Arg1
; CV# bits 8-9 in W (lsb)
;
CV_to_addr	andlw	b'00000011' 		;check hi bits; have to be 0
		btfss	STATUS,	Z	
		retlw	0xFF
;
		movlw	d'6'
		subwf	Arg1,	w		; a-6
		btfsc	STATUS,	C		; CV <= 6 (a <= 5)
		goto	l_cva_8
		addlw	d'6'			; get 'a' back, it's the address
		return
;
l_cva_8		movlw	d'8'
		subwf	Arg1,	w		; a-8
		btfsc	STATUS, C		; CV <= 8 (a <= 7)
		goto	l_cva_1
		addlw	h'F8'			; mark as special reg. (read only)
		return
;
l_cva_1		movlw	d'8'			; CV9 - CV11 (8 <= a <= 10)
		subwf	Arg1,	w		; a-8
		btfss	STATUS,	C
		retlw	0xFF
		addlw	-3			; a-11
		btfsc	STATUS, C
		goto	l_cva_2
		addlw	d'9'			; 9+(a-11) == a-2
		return
;
; CV17-19 (16 <= a <=18)
l_cva_2		movlw	d'16'
		subwf	Arg1,	w		; a-16 
		btfss	STATUS,	C
		retlw	0xFF			; CV < 17
		addlw	-3			; a-19
		btfsc	STATUS, C
		goto	l_cva_3
		addlw	d'12'			; 12+(a-19) == a-7
		return
;
; CV21-25
l_cva_3		movlw	d'20'
		subwf	Arg1,	w		; a-20
		btfss	STATUS,	C
		retlw	0xFF			; CV < 21
		addlw	-5			; a-25
		btfsc	STATUS, C
		goto	l_cva_4
		addlw	d'17'			; 17+(a-25) == a-8
		return
;
; CV29-30
l_cva_4		movlw	d'28'
		subwf	Arg1,	w		; a-28
		btfss	STATUS,	C
		retlw	0xFF			; CV < 29
		addlw	-2			; a-30
		btfsc	STATUS, C
		goto	l_cva_5
		addlw	d'19'			; 19+(a-30) == a-11
		return
;
; CV33-37 = 19-23
l_cva_5		movlw	d'32'
		subwf	Arg1,	w		; a-32
		btfss	STATUS,	C
		retlw	0xFF			; CV < 29
		addlw	-5			; a-37
		btfsc	STATUS, C
		goto	l_cva_6
		addlw	d'24'			; 24+(a-37) == a-13
		return
; CV49 - 60 = 24-35		
l_cva_6		movlw	d'48'
		subwf	Arg1,	w		; a - 48
		btfss	STATUS, C
		retlw	0xFF			; CV < 49
		addlw	-d'12'			; a - 60
		btfsc	STATUS, C
		goto	l_cva_7
		addlw	d'36'			; 36+(a-60) == a-24
		return
; CV67-94  == 36-63					
l_cva_7		movlw	d'66'
		subwf	Arg1,	w		; a-66
		btfss	STATUS,	C
		retlw	0xFF			; CV < 67
		addlw	-d'28'			; a-94
		btfsc	STATUS, C
		retlw	0xFF			; no more CVs supported
		addlw	d'64'			; 64+(a-94) == a-30
		return	
	
	
;----------------------------------------
;	DATA EEPROM preset values
;
	org	0x2100
	de	3		;PRIM_Addr   	;CV1
	de	1		;V_Start	;CV2  Use 1 here with BEMF
	de	5		;ACC_Rate	;CV3  A nice value. Set to 0 for no accel
	de	5		;DEC_Rate	;CV4  A nice value. Set to 0 for no decel
	de	1		;V_High		;CV5
	de	.75		;V_Mid		;CV6  A reasonable value if speed table off
				;		      Set to 0 for a linear response
; CV7-CV8 implemented as constants
	de	0		;PWMTot		;CV9  = 6  (not used in this version)
	de	h'FF'		;EMFCut		;CV10	   Reduce if lower BEMF cutoff wanted
	de	0		;Packet_TO	;CV11 = 8  0 is off, else value in seconds
; CV14-16 reserved
	de	0		;ExtAddr1	;CV17 = 9
	de	0		;ExtAddr2	;CV18
	de	0		;Consist_addr	;CV19 = 11
; CV20 reserved
	de	0		;CAct_F1_F8	;CV21 = 12
	de	0		;CAct_LGT	;CV22
	de	0		;Accel_Adj	;CV23
	de	0		;Decel_Adj	;CV24
	de	1		;CABSPD_Step	;CV25 = 16
; CV26-28 reserved
	de	B'00010110'	;Config_data_1	;CV29 = 17  Speed table on
	de	0		;Err_Info	;CV30 = 18
; CV31-32 n/a
	de	OUT1_msk	;FL_loc		;CV33 = 19  Output 1
	de	OUT2_msk	;RL_loc		;CV34	    Output 2
	de	OUT3_msk	;F1_loc		;CV35	    Output 3
	de	OUT4_msk	;F2_loc		;CV36	    Output 4
	de	OUT5_msk	;F3_loc		;CV37 = 23  Output 5
;
	de	B'00010000'	;FL_Effect	;CV49 = 24  Front light
	de	B'00001000'	;RL_Effect	;CV50	    Rear light 
	de	0		;F1_Effect	;CV51
	de	0		;F2_Effect	;CV52
	de	0		;F3_Effect	;CV53
	de	h'30'		;PWM_Mode	;CV54  HF PWM, BEMF on, no low speed LF.
	de	h'80'		;Ki		;CV55  A conservative value, may be increased
	de	h'80'		;Kp		;CV56  Reduce for coreless motors
	de	d'166'		;Kfr		;CV57  Play around with this
	de	0		;Reserv1	;CV58
	de	0		;Reserv2	;CV59
	de	0		;Reserv3	;CV60  = 35
;
				;SPD_Tbl:1C 	;CV67 - 94 = 36 - 63 (28 values)
				;A roughly parabolic speed curve
				;Good control at low speeds
	de	d'1'		;CV67
	de	d'2'		;CV68
	de	d'3'		;CV69
	de	d'5'		;CV70
	de	d'8'		;CV71
	de	d'12'		;CV72
	de	d'16'		;CV73
	de	d'21'		;CV74
	de	d'26'		;CV75
	de	d'33'		;CV76
	de	d'39'		;CV77
	de	d'47'		;CV78
	de	d'55'		;CV79
	de	d'64'		;CV80
	de	d'73'		;CV81
	de	d'83'		;CV82
	de	d'94'		;CV83
	de	d'105'		;CV84
	de	d'117'		;CV85
	de	d'129'		;CV86
	de	d'143'		;CV87
	de	d'156'		;CV88
	de	d'171'		;CV89
	de	d'186'		;CV90
	de	d'202'		;CV91
	de	d'218'		;CV92
	de	d'235'		;CV93
	de	d'255'		;CV94
	

	END


