; Ver 1.0vDate:	20. 4.2007
; This is a basic DCC decoder with the 12F629
;  CV1=3,CV2=20,CV3+4=1,CV5=200,CV9=0,CV19=0,CV29=2,CV65=20

; #define testing 1
; #define test3ff

	LIST	p=12F629	; target processor
	__CONFIG  _BODEN_ON & _CP_OFF & _WDT_OFF & _MCLRE_OFF & _INTRC_OSC_NOCLKOUT & _PWRTE_ON

;==========================================================================
;
;       Register Definitions
;
;==========================================================================

W	EQU	H'0000'
F	EQU	H'0001'

;----- Register Files------------------------------------------------------

INDF	EQU	H'0000'
TMR0	EQU	H'0001'
PCL	EQU	H'0002'
STATUS	EQU	H'0003'
FSR	EQU	H'0004'
GPIO	EQU	H'0005'

PCLATH	EQU	H'000A'
INTCON	EQU	H'000B'
PIR1	EQU	H'000C'

TMR1L	EQU	H'000E'		
TMR1H	EQU	H'000F'		
T1CON	EQU	H'0010'		
CMCON	EQU	H'0019'		
OPTION_REG	EQU	H'0081'
TRISIO	EQU	H'0085'
PIE1	EQU	H'008C'
PCON	EQU	H'008E'
OSCCAL	EQU	H'0090'
WPU	EQU	H'0095'
IOCB	EQU	H'0096'
IOC	EQU	H'0096'

VRCON	EQU	H'0099'
EEDATA	EQU	H'009A'	
EEADR	EQU	H'009B'	
EECON1	EQU	H'009C'
EECON2	EQU	H'009D'

;----- STATUS Bits --------------------------------------------------------

IRP	EQU	H'0007'
RP1	EQU	H'0006'
RP0	EQU	H'0005'
NOT_TO	EQU	H'0004'
NOT_PD	EQU	H'0003'
Z	EQU	H'0002'
DC	EQU	H'0001'
C	EQU	H'0000'

;----- GPIO Bits --------------------------------------------------------

GP5	EQU	H'0005'
GPIO5	EQU	H'0005'
GP4	EQU	H'0004'
GPIO4	EQU	H'0004'
GP3	EQU	H'0003'
GPIO3	EQU	H'0003'
GP2	EQU	H'0002'
GPIO2	EQU	H'0002'
GP1	EQU	H'0001'
GPIO1	EQU	H'0001'
GP0	EQU	H'0000'
GPIO0	EQU	H'0000'

;----- INTCON Bits --------------------------------------------------------

GIE	EQU	H'0007'
PEIE	EQU	H'0006'
T0IE	EQU	H'0005'
INTE	EQU	H'0004'
GPIE	EQU	H'0003'
T0IF	EQU	H'0002'
INTF	EQU	H'0001'
GPIF	EQU	H'0000'

;----- PIR1 Bits ----------------------------------------------------------

EEIF	EQU	H'0007'
ADIF	EQU	H'0006'
CMIF	EQU	H'0003'
T1IF	EQU	H'0000'
TMR1IF	EQU	H'0000'

;----- T1CON Bits ---------------------------------------------------------

TMR1GE	EQU	H'0006'
T1CKPS1	EQU	H'0005'
T1CKPS0	EQU	H'0004'
T1OSCEN	EQU	H'0003'
NOT_T1SYNC	EQU	H'0002'
TMR1CS	EQU	H'0001'
TMR1ON	EQU	H'0000'

;----- COMCON Bits --------------------------------------------------------

COUT	EQU	H'0006'
CINV	EQU	H'0004'
CIS	EQU	H'0003'
CM2	EQU	H'0002'
CM1	EQU	H'0001'
CM0	EQU	H'0000'

;----- OPTION Bits --------------------------------------------------------

NOT_GPPU	EQU	H'0007'
INTEDG	EQU	H'0006'
T0CS	EQU	H'0005'
T0SE	EQU	H'0004'
PSA	EQU	H'0003'
PS2	EQU	H'0002'
PS1	EQU	H'0001'
PS0	EQU	H'0000'

;----- PIE1 Bits ----------------------------------------------------------

EEIE	EQU	H'0007'
ADIE	EQU	H'0006'
CMIE	EQU	H'0003'
T1IE	EQU	H'0000'
TMR1IE	EQU	H'0000'

;----- PCON Bits ----------------------------------------------------------

NOT_POR	EQU	H'0001'
NOT_BOD	EQU	H'0000'

;----- OSCCAL Bits --------------------------------------------------------

CAL5	EQU	H'0007'
CAL4	EQU	H'0006'
CAL3	EQU	H'0005'
CAL2	EQU	H'0004'
CAL1	EQU	H'0003'
CAL0	EQU	H'0002'

;----- IOCB Bits --------------------------------------------------------

IOCB5	EQU	H'0005'
IOCB4	EQU	H'0004'
IOCB3	EQU	H'0003'
IOCB2	EQU	H'0002'
IOCB1	EQU	H'0001'
IOCB0	EQU	H'0000'

;----- IOC Bits --------------------------------------------------------

IOC5	EQU	H'0005'
IOC4	EQU	H'0004'
IOC3	EQU	H'0003'
IOC2	EQU	H'0002'
IOC1	EQU	H'0001'
IOC0	EQU	H'0000'

;----- VRCON Bits ---------------------------------------------------------

VREN	EQU	H'0007'
VRR	EQU	H'0005'
VR3	EQU	H'0003'
VR2	EQU	H'0002'
VR1	EQU	H'0001'
VR0	EQU	H'0000'

;----- EECON1 -------------------------------------------------------------

WRERR	EQU	H'0003'
WREN	EQU	H'0002'
WR	EQU	H'0001'
RD	EQU	H'0000'

;==========================================================================
;
;       RAM Definition
;
;==========================================================================

        __MAXRAM H'FF'
        __BADRAM H'06'-H'09', H'0D', H'11'-H'18', H'1A'-H'1F', H'60'-H'7F'
        __BADRAM H'86'-H'89', H'8D', H'8F', H'91'-H'94', H'97'-H'98', H'9E'-H'9F', H'E0'-H'FF'

;==========================================================================
;
;       Configuration Bits
;
;==========================================================================

_CPD_ON		EQU	H'3EFF'
_CPD_OFF	EQU	H'3FFF'
_CP_ON		EQU	H'3F7F'
_CP_OFF		EQU	H'3FFF'
_BODEN_ON	EQU	H'3FFF'
_BODEN_OFF	EQU	H'3FBF'
_MCLRE_ON	EQU	H'3FFF'
_MCLRE_OFF	EQU	H'3FDF'
_PWRTE_OFF	EQU	H'3FFF'
_PWRTE_ON	EQU	H'3FEF'
_WDT_ON		EQU	H'3FFF'
_WDT_OFF	EQU	H'3FF7'
_LP_OSC		EQU	H'3FF8'
_XT_OSC		EQU	H'3FF9'
_HS_OSC		EQU	H'3FFA'
_EC_OSC		EQU	H'3FFB'
_INTRC_OSC_NOCLKOUT	EQU	H'3FFC'
_INTRC_OSC_CLKOUT  	EQU	H'3FFD'
_EXTRC_OSC_NOCLKOUT	EQU	H'3FFE'
_EXTRC_OSC_CLKOUT	EQU	H'3FFF'

;*************MACROS for convenience*****************
; A > B
gthan	macro	A,B
	movf	A,W
	subwf	B,W
	btfsc	STATUS,C
	endm
; A >= B
gequal	macro	A,B
	movf	B,W
	subwf	A,W
	btfss	STATUS,C
	endm
; A < B
lthan	macro	A,B
	movf	B,W
	subwf	A,W
	btfsc	STATUS,C
	endm
; A <= B
lequal	macro	A,B
	movf	A,W
	subwf	B,W
	btfss	STATUS,C
	endm
;***************** User definable variables in this block ***********************************

#define cv9default	B'10100000'	; the last three bits determine the PWM. Default to 2kHz
;			       ^^^ 
; Bit pattern	PWM time	PWM frequency
; 000		512us		-> 2kHz
; 001		1ms		-> 1kHz
; 010		2ms		-> 500Hz
; 011		4ms		-> 250Hz
; 100		8ms		-> 125Hz
; 101		16ms		-> 62Hz
; 110		32ms		-> 31Hz
; 111		64ms		-> 15Hz


#define cv19default 0		; no consist as default
#define cv29default B'00000010'	; CV29 BIT0=0 normal dir
				; CV29 BIT1=0 FL in speed packet and Speed14.
				; If set to one Speed28 is set.

#define AckTime  .40		; Acknowledge time

;--------------- Do not change anything below this line ----------------------------------------
;-----------------------------------------------------------------------------------------------

;EEprom location
#define	eeprombase	0
#define CV1adr		eeprombase
#define CV2adr		eeprombase + .1
#define CV3adr		eeprombase + .2
#define CV4adr		eeprombase + .3
#define CV5adr		eeprombase + .4
#define CV9adr		eeprombase + .5
#define CV19adr		eeprombase + .6
#define CV29adr		eeprombase + .7
#define CV64adr		eeprombase + .8
#define CV65adr		eeprombase + .9
#define CV17adr		eeprombase + .10
#define CV18adr		eeprombase + .11
;******************************************************
; VARIABLES

#define forward		B'00000100'	; Masks for Direction of MOTMASK
#define backward	B'00000010'	; GPIO 1 and GPIO 2 are the outputs
					; hence GPIO 1 is forward Output

#define nopreamb	D'19'		; No. of preamble bits = 20

;Bit positions
#define norm_rev	.0		; CV29 Bit0 normal or reverse operation
#define fl_control	.1		; CV29 Bit1 FL control = 0 FL in speed14,=1 speed28 FL in function one
#define long_adr	.5
#define cv21_bit	.6		; CV21 if 1 than consist adr controls Functions

#define defaultstep D'28'		; default step size is Speed28

; Pin connection of 12CE519 decoder
; gp0 = FL out
; gp1 = Motor out and adress add input on start
; gp2 = Motor out and basekick add on start
; 
; gp3 = MCLR
; gp4 = input
; gp5 = F1

#define gp0	0		; FL output
#define gp1	1		; motor out1
#define gp2	2		; motor out2
#define gp3	3		; MCLR
#define gp4	4		; data in
#define gp5	5		; F1

#define FN0	gp0		; Bit Position MOTMASK
#define FN1	gp5
;******************************************************
; Bit Positions in CONFIGa
#define bitrec		0	; What value has just rec. bit
#define CVaccessBit	1	; Two consecutive packets to be send
#define rev_bit		2	; watch CV3/4 when changing direction set by new speed packet
#define calc_rev_bit	3	; we came from speed macro and do the rev calc
#define kickstartflag	4	; Do we use Kick start ?
#define updown		5	; Count up or down PWM
#define resetflag	6	; Signal reset for service mode
#define smflag		7	; Service Mode Flag
; Bit Positions in CONFIG1
#define cst_rev		0	; Consist Reverse Mode
#define cst_mode	1	; consist mode
#define cst_adr		2	; valid consist adr
; Bit Positions in CONFIG2
#define FN_ff		0	; smer jazdy
#define wait_stop	1	; min cas v zast.
;******************************************************
; RAM locations. EEPROM routine uses 0x1A  to 0x1F !

#ifdef testing
		cblock	0x20
CONFIGa				; Contains various bits to test
MOTMASK				; Motormask for direction
PWM				; How long is motor on
ENDVAL				; Offset to byte check routines
PRECOUNT			; Preamble counter
STATE				; Where are we in the DCC package
DATA1				; First transm. byte
DATA2				; Second trans. byte
DATA3				; Third transm. byte
DATA4				; Fourth transm. byte
DATA5				; Scratch Register
SAMPLES				; How many samples taken for one or zero
TEMP				; Temporarly scratch reg
				; the following two are for EEPROM routines
TEMP1
COUNTER    			; Bit counter for serial transfer

SPEEDSTEP
ACC_TIMER
ACC_COUNT
CVACCESSRATE
SPEEDNEW
TIMEOUTCOUNT
CV2
CV29
CV65
NEWSPEED			; when changing dir save newspeed for later
MYEEDATA
CONFIG1
; only for testing
CV1				; Store actual adress of decoder
CV3
CV4
CV5
CV9
CV19
		endc
#endif
#ifndef testing
		cblock	0x20
CONFIGa				; Contains various bits to test
MOTMASK				; Motormask for direction
PWM				; How long is motor on
ENDVAL				; Offset to byte check routines
PRECOUNT			; Preamble counter
STATE				; Where are we in the DCC package
DATA1				; First transm. byte
DATA2				; Second trans. byte
DATA3				; Third transm. byte
DATA4				; Fourth transm. byte
DATA5				; Scratch Register
SAMPLES				; How many samples taken for one or zero
TEMP				; Temporarly scratch reg
				; the following two are for EEPROM routines
TEMP1
COUNTER    			; Bit counter for serial transfer

SPEEDSTEP
ACC_TIMER
ACC_COUNT
CVACCESSRATE
SPEEDNEW
TIMEOUTCOUNT
CV2
CV29
CV64
CV65
NEWSPEED			; when changing dir save newspeed for later
MYEEDATA
CONFIG1
lamp_sim
lamp_temp
WAIT_REV
CONFIG2
CONFIG3
		endc
#endif
;************************************************************************************************
;#ifdef test3ff
	org	0x3ff
	retlw	0x6c
;#endif
;************************************************************************************************
					; Macro for generating the PWM, ACC and DEC
speed	macro	jumpto
	local	on,down11,down12,up1
	local	off,exit7,exit9,speed_on,down,carry_on

	decfsz	ACC_TIMER,F	;9	Outer Timer Loop Acceleration/deceleration value
	goto	speed_on	;10,11	Do PWM if ACC/DEC Value not reached
	decfsz	TIMEOUTCOUNT,F	;12	For ANALOG mode. Timeoutcount is cleared in state machine.
	goto	carry_on	;13,14
	goto	ANALOG		;14,15

carry_on:
	btfsc	CONFIGa,kickstartflag	; slowing down from CV65+CV2-->CV2
	goto	down

	movf	PWM,W		;15	Read actual PWM value
	xorwf	SPEEDNEW,W	;16	Is it same as Newspeed ?
	btfsc	STATUS,Z	;17	
	goto	exit7		;18,19	No then exit with testing the Reverse Bit
	decfsz	ACC_COUNT,F	;19	Decrement The ACC/DEC Timer Counter, i.e. Slope of ACC/DEC
	goto	exit9		;20,21	If not just exit
	btfss	CONFIGa,updown	;21	Do we ACC or DEC ?
	goto	down		;22,23	We slow down if we go there

	movf	PWM,F		; 	did we stop before ?
	btfss	STATUS,Z	;	if not just inc PWM and speed up again
	goto	up1
	movf	CV2,W		;	get CV2
	addwf	CV65,W		;	do kickstart
	movwf	PWM		;	store in PWM
	bsf	CONFIGa,kickstartflag	; and set the flag to slow down again
up1:
	incf	PWM,F		;	We speed up here. The only incf PWM anywhere !
	movf	CVACCESSRATE,W	;	Read the ACC Rate/Slope
	movwf	ACC_COUNT	;	Store in temp count reg
	goto	jumpto		;	out of sync here every 255 ACC_TIMER cycles


down:
	decf	PWM,F		;24	We slow down here. The only decf PWM anywhere !
	movf	CV2,W		;	PWM < CV2 ?
	subwf	PWM,W
	btfsc	STATUS,C
	goto	down11		;	No, just carry on
	btfsc	CONFIGa,kickstartflag	; Do we slow down from Kickstart ?
	goto	down12		
	clrf	PWM		;	Stop when PWM < CV2 AND !kickstartflag
down12:
	bcf	CONFIGa,kickstartflag	; clear kickstartflag when CV2 is reached after CV2+CV65	
down11:
	movf	CVACCESSRATE,W	;	Read the ACC Rate
	movwf	ACC_COUNT	;	Store in temp count reg
	goto	jumpto		;	Out of sync here every 255 ACC_TIMER cycles

exit7:
	btfsc	CONFIGa,rev_bit	;20	changing direction ?
	goto	calc_rev_speed	;21	if yes go and decode it (no cycle count required)
exit9:
	nop			;22
	nop			;23
	goto	jumpto		;24,25	end	<- out of sync here every 255 ACC_TIMER cycles !

; PWM section of SPEED macro
speed_on:
	bcf	STATUS,C	;12	clear carry because we use it
	movf	TMR0,W		;13	read timer
	addwf	PWM,W		;14	plus our PWM > 0xFF ?
	btfss	STATUS,C	;15	if yes carry is set and turn motor on
	goto	off		;16,17

on:
	movf	MOTMASK,W	;17	direction in motormask
	movwf	GPIO		;18	send it to output
	nop			;19
	goto	jumpto		;20,21	end, important to be in sync here

off:
	movlw	B'00111001'
	andwf	GPIO,F
	;bcf	GPIO,gp1	;18	to turn motor off,
	;bcf	GPIO,gp2	;19	this will reduce heating in the power bridge
	goto	jumpto		;20,21	end, important to be in sync here

	endm
;************************************************************************************************************
;************* Here we start ! ******************************************************************************

	org	0x0
	bsf	STATUS,RP0
	call	0x3FF
	movwf	OSCCAL		;put calibration value into OSCCAL
	bcf	STATUS,RP0

	goto	dccsetup	;jump over to EEPROM routine and other stuff

;************************************************************************************************************
				;Value checks for correct bit and jumps to where we are in DCC package
				;Computed goto has to reside in lower half of page, that's why it is here

Value:
	clrf	SAMPLES		; 6
	movf	STATE,W		; 7
	addwf	PCL,F		; 8
				;Jump table to routines from where we do a RETURN !
				;cycl. to here ->  cycl for rout.
	goto	waitn		; 9,10
	goto	waitlo
	goto	testlo
; First byte
	goto	bitset
	goto	lastv
	goto	bitset
	goto	lastv
	goto	bitset
	goto	lastv
	goto	bitset
	goto	lastv
	goto	bitset
	goto	lastv
	goto	bitset
	goto	lastv
	goto	bitset
	goto	lastv
	goto	bitset
	goto	lastx		; Byte received
	goto	end11		; Check first half bit of byte
	goto	end12		; Check second half bit of byte
	goto	end21
	goto	end22
	goto	end31
	goto	end32
	goto	end41
	goto	end42
	goto	end51
	goto	end52

;************************************************************************************************************
calc_rev_speed:
	movf	NEWSPEED,W		;get saved speed value
	movwf	DATA2			;into data2 for decoding
	bcf	CONFIGa,rev_bit		;reset change dir flag
	bsf	CONFIGa,calc_rev_bit	;set calc rev flag
	call	decode1
	goto	conthi
;************* setup sets all what is ness. *****************************************************************

dccsetup:
	movlw	0x20		; bottom of RAM
	movwf	FSR
Next1:
	clrf	INDF
	incf	FSR,F
	movlw	0x5f		; end of ram
	subwf	FSR,W
	btfss	STATUS,C
	goto	Next1
	clrf	INDF


	movlw	nopreamb	; Preamble = 20 half bits
	movwf	PRECOUNT
	movlw	.2
	movwf	ACC_COUNT	; Preset ACC_COUNT

	clrf	TIMEOUTCOUNT	; clear packet time out
				; call master init, read contents of EEPROM
	call	ChkPowerUp
	call	ReadNMRA


	movlw	0x7
	movwf	CMCON		; set GP2:0 to digital I/O
	bsf	STATUS,RP0
	
	; GPIO      543210
	movlw	B'00011000'	; gp0,1,2,5 are outputs
	movwf	TRISIO		; gp2 dcc datain
	bcf	STATUS,RP0

	clrf	GPIO
	;bcf	GPIO,gp1	; Turn motor off
	;bcf	GPIO,gp2	;

;************************************************************************************************************
; High Bit Level Section
starthi:
	call	Value		;4,5
conthi:
	btfss	GPIO,gp4	;1	HighLevel ?
	goto	startlo		;2,3	No
	incf	SAMPLES,F	;3	Increment SAMPLES Counter
	bcf	CONFIGa,bitrec	;4	clear BITREC
	movlw	0xFD		;5	two or more samples are a ZERO
	addwf	SAMPLES,W	;6	add the received pulses
	btfss	STATUS,C	;7	
	bsf	CONFIGa,bitrec	;8	less then two SAMPLES is a one
	speed	conthi
;----------------------------------------------------------------------------------------------------
; Low Level Bit Section
startlo:
	call	Value		;4,5
contlo:
	btfsc	GPIO,gp4	;1
	goto	starthi		;2,3
	incf	SAMPLES,F	;3
	bcf	CONFIGa,bitrec	;4
	movlw	0xFD		;5
	addwf	SAMPLES,W	;6
	btfss	STATUS,C	;7
	bsf	CONFIGa,bitrec	;8
	speed	contlo
;----------------------------------------------------------------------------------------------------
				; Frame error in packet
frameerr:
	movlw	nopreamb
	movwf	PRECOUNT
	clrf	STATE
	clrf	ENDVAL
	clrf	DATA1
	clrf	DATA2
	clrf	DATA3
	clrf	DATA4
	clrf	DATA5
	clrf	SAMPLES
	movlw	0xF0		; cut of low nibble in CONFIG Byte
	andwf	CONFIGa,F
	return

;************************************************************************************************************
				; waitn waits for 20 half bits of the preamble

waitn:	
	nop					;11
	nop						;12
	btfss	CONFIGa,bitrec	;13   there are only ones in preamble
	goto	waitn1		;14,15
	decfsz	PRECOUNT,F	;16
	goto	ret4		;16,17
	movlw	nopreamb	;17
	movwf	PRECOUNT	;18
	incf	STATE,F		;19
	return			;20,21
ret4:
	nop			;18
	nop			;19
	return			;20,21

waitn1:
	movlw	nopreamb	;16
	movwf	PRECOUNT	;17
	nop			;18
	nop			;19
	return			;20,21

;************************************************************************************************************
				; waitlo waits for low half bit after preamble

waitlo:	
	nop			;11
	nop			;12
	nop			;13

waitlo1:
	nop			;14
	nop			;15
	nop			;16
	btfsc	CONFIGa,bitrec	;17	must be a zero
	goto	ret5		;18	might be long preamble
	incf	STATE,F		;19
ret5:
	return			;20,21

;************************************************************************************************************
				; testlo test second half bit of start bit

testlo:	
	nop			;11
	nop			;12
	nop			;13
	nop			;14
	nop			;15
	nop			;16
	btfsc	CONFIGa,bitrec	;17	must be a zero
	goto	frameerr	;18
	incf	STATE,F		;19
	return			;20,21

;************************************************************************************************************
				; bitset takes first half bit of a bit in the byte

bitset:	
	incf	STATE,F		;11
	bcf	STATUS,C	;12
	btfsc	CONFIGa,bitrec	;13
	bsf	STATUS,C	;14
	rlf	DATA5,F		;15
	nop			;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

;************************************************************************************************************
				; lastv checks that first half bit = second half bit
lastv:	
	nop			;11
	nop			;12
	nop			;13
	incf	STATE,F		;14
	btfss	CONFIGa,bitrec	;15
	goto	lastv1		;16,17
	btfss	DATA5,0		;17
	goto	frameerr	;18,19	error if not equal
	nop			;19
	return			;20,21

lastv1:
	btfsc	DATA5,0		;18
	goto	frameerr	;19	error if not equal
	return			;20,21
;************************************************************************************************************
				; lastx checks that first half bit = second half bit
				; and sets offset to byte check routine
lastx:	
	incf	ENDVAL,F	;11
	movf	ENDVAL,W	;12
	addwf	STATE,F		;13
	nop			;14
	btfss	CONFIGa,bitrec	;15
	goto	lastx1		;16,17
	btfss	DATA5,0		;17
	goto	frameerr	;18	error if not equal
	nop			;19
	return			;20,21

lastx1:
	btfsc	DATA5,0		;18
	goto	frameerr	;19	error if not equal
	return			;20,21

;************************************************************************************************************
				; end11 end of first byte there must be zero
				; end11 first half bit. end12 second half bit
end11:
	btfsc	CONFIGa,bitrec	;11
	goto	frameerr	;12,13
	incf	STATE,F		;13
	incf	ENDVAL,F	;14
	nop			;15
	nop			;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

end12:	
	btfsc	CONFIGa,bitrec	;11
	goto	frameerr	;12
	movf	DATA5,W		;13
	movwf	DATA1		;14
	movlw	0x03		;15	point STATE to beginning of jump table
	movwf	STATE		;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

;************************************************************************************************************
				; end21 end of second byte there must be zero
				; end22 first half bit. end22 second half bit
end21:	
	btfsc	CONFIGa,bitrec	;11
	goto	frameerr	;12
	incf	STATE,F		;13
	incf	ENDVAL,F	;14
	nop			;15
	nop			;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

end22:	
	btfsc	CONFIGa,bitrec	;11
	goto	frameerr	;12
	movf	DATA5,W		;13
	movwf	DATA2		;14
	movlw	0x03		;15	point STATE to beginning of jump table
	movwf	STATE		;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

;************************************************************************************************************
				; end31 end of third byte there must be a one
				; end32 first half bit. end32 second half bit

end31:	
	incf	STATE,F		;11
	incf	ENDVAL,F	;12
	nop			;13
	nop			;14
	nop			;15
	nop			;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

end32:	
	movf	DATA5,W		;11
	movwf	DATA3		;12
	btfsc	CONFIGa,bitrec	;13	if 0 other bytes will follow
	goto	end32x		;14,15
	movlw	0x03		;15	point STATE to beginning of jump table
	movwf	STATE		;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

end32x: 
	clrf	STATE		;16	reset STATE for next preamble
	clrf	ENDVAL		;17
	clrf	DATA4		;18
	clrf	DATA5		;19
	goto	decode		;21,22	no need here for precise cycle counting, because we decode now
;************************************************************************************************************
				; end41 end of third byte there must be a one
				; end42 first half bit. end42 second half bit

end41:	
	incf	STATE,F		;11
	incf	ENDVAL,F	;12
	nop			;13
	nop			;14
	nop			;15
	nop			;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

end42:	
	movf	DATA5,W		;11
	movwf	DATA4		;12
	btfsc	CONFIGa,bitrec	;13	if 0 other bytes will follow
	goto	end42x		;14,15
	movlw	0x03		;15	point STATE to beginning of jump table
	movwf	STATE		;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

end42x: 
	clrf	STATE		;16	reset STATE for next preamble
	clrf	ENDVAL		;17
	clrf	DATA5		;18
	goto	decode		;20,21	no need to count, because we decode now
;************************************************************************************************************
				; end51 end of third byte there must be a one
				; end52 first half bit. end52 second half bit

end51:	
	btfss	CONFIGa,bitrec	;11
	goto	frameerr	;12,13
	incf	STATE,F		;13
	incf	ENDVAL,F	;14
	nop			;15
	nop			;16
	nop			;17
	nop			;18
	nop			;19
	return			;20,21

end52:
	btfss	CONFIGa,bitrec	;11	test correct ending
	goto	frameerr	;12,13
	clrf	STATE		;13	reset STATE for next preamble
	clrf	ENDVAL		;14
	goto	decode
;************************************************************************************************************
;*****************************************************************************************************************

EEPROM_WRITE:
; Adress in W, DATA in MYEEDATA
	bsf	STATUS,RP0
	movwf	EEADR
	bcf	STATUS,RP0
	movf	MYEEDATA,W
	bsf	STATUS,RP0
	movwf	EEDATA
	bsf	EECON1,WREN
	movlw	0x55
	movwf	EECON2
	movlw	0xAA
	movwf	EECON2
	bsf	EECON1,WR
wait_ee_write:
	btfsc	EECON1,WR
	goto	wait_ee_write

	bcf	STATUS,RP0
	return
EEPROM_READ:
; Adress in W
; return data in W
	bsf	STATUS,RP0
	movwf	EEADR
	bsf	EECON1,RD
	movf	EEDATA,W
	bcf	STATUS,RP0
	return

;************************************************************************************************************
ANALOG:
	movlw	B'00111111'	; all input
	bsf	STATUS,RP0
	movwf	TRISIO		; tristate I/O
	bcf	STATUS,RP0
ANALOG_1:
	goto	ANALOG_1	; stay here till voltage drop occurs
;************************************************************************************************************
ReadNMRA:
#ifdef testing
	clrw
	movwf	CV19
	movwf	CV9
	movlw	.5
	movwf	CV1
	movlw	.20
	movwf	CV2
	movlw	.1
	movwf	CV3
	movwf	CV4
	movlw	0xff
	movwf	CV5
	movlw	.20
	movwf	CV65
	movlw	.2
	movwf	CV29
#endif ; be executed.
#ifndef testing
	movlw	CV2adr
	call	EEPROM_READ
	movwf	CV2
	movlw	CV64adr
	call	EEPROM_READ
	movwf	CV64
	movlw	CV65adr
	call	EEPROM_READ
	movwf	CV65
	movlw	CV29adr
	call	EEPROM_READ
	movwf	CV29
#endif ; be executed.

EE_exit:
#ifndef testing
	movlw	CV9adr
	call	EEPROM_READ
#endif
#ifdef testing
	movf	CV9,W
#endif
	andlw	B'00000111'
	iorlw	B'11000000'
; Timer setting here
	bsf	STATUS,RP0
	movwf	OPTION_REG ; Prescaler in TIMER0
	bcf	STATUS,RP0

divide8x8:
#ifndef testing
	movlw	CV5adr
	call	EEPROM_READ
	movwf	SPEEDSTEP
#endif
#ifdef testing
	movf	CV5,W
	movwf	SPEEDSTEP
#endif
	movf	CV2,W
	subwf	SPEEDSTEP,F
	movlw	defaultstep		; Divide by 28 steps default
	movwf	TEMP
;----------------------------------------
FXD0808U:				; Divide unsigned 8x8 from Microchip
	clrf	DATA1
	movlw	8
	movwf	COUNTER
LOOPU0808A:
	rlf	SPEEDSTEP,W
	rlf	DATA1, F
	movf	TEMP,W
	subwf	DATA1, F
	btfsc	STATUS,C
	goto	UOK88A
	addwf	DATA1, F
	bcf	STATUS,C
UOK88A:
	rlf	SPEEDSTEP, F
	decfsz	COUNTER, F
	goto	LOOPU0808A
	goto	exit_bank1		; start sampling
;************************************************************************************************************
;------- CV in W. Returns EEPROM location in W -------------------
GetEEPROMaddress:
	xorlw .0			; CV1 = eeprombase (see *.h)
	btfsc STATUS,Z
	retlw eeprombase
	xorlw .1			; CV2 = +1
	btfsc STATUS,Z
	retlw eeprombase + .1
	xorlw .3			; CV3 = +2
	btfsc STATUS,Z
	retlw eeprombase + .2
	xorlw .1			; CV4 = +3
	btfsc STATUS,Z
	retlw eeprombase + .3
	xorlw .7			; CV5 = +4
	btfsc STATUS,Z
	retlw eeprombase + .4
	xorlw .12			; CV9 = +5
	btfsc STATUS,Z
	retlw eeprombase + .5
	xorlw .26			; CV19 = +6
	btfsc STATUS,Z
	retlw eeprombase + .6
	xorlw .14			; CV29 = +7
	btfsc STATUS,Z
	retlw eeprombase + .7
	xorlw .35			; CV64 = +8
	btfsc STATUS,Z
	retlw eeprombase + .8
	xorlw .127			; CV65 = +9
	btfsc STATUS,Z
	retlw eeprombase + .9
	xorlw .80			; CV17 = +10
	btfsc STATUS,Z
	retlw eeprombase + .10
	xorlw .1			; CV18 = +11
	btfsc STATUS,Z
	retlw eeprombase + .11

	retlw 0xFF			; if you came here return ERROR
;******************CV Access group*******************************
					; D2         D3         D4
CVaccess:				; 1110CCAA 0 AAAAAAAA 0 DDDDDDDD
	btfss	CONFIG1,cst_mode	; chk if basline address ok
	goto	CVaccess1
	btfsc	CONFIG1,cst_adr		; if consist adr then no CVaccess
	goto	exit_bank1
CVaccess1:
	btfsc	CONFIGa,CVaccessBit	; second packet received ?
	goto	DoCV
	bsf	CONFIGa,CVaccessBit
	goto	exit_bank1	
DoCV:
	bcf	CONFIGa,CVaccessBit
Test_CV:
	movf	DATA3,W			; ignore bit9 and 10
	call	GetEEPROMaddress	; get EEPROM loc. or error
	movwf	TEMP
	incfsz	TEMP,W			; Valid CV# ?
	goto	carry_on_cv_prog_adr
	goto	exit_bank1		; No valid CV
carry_on_cv_prog_adr:
	movf	DATA2,W
	andlw	B'00001100'
	xorlw	B'00001100'		; Write ?
	btfsc	STATUS,Z
	goto	cv_prog
	xorlw	B'00001000'
	btfsc	STATUS,Z
	goto	cv_verify
	goto	exit_bank1		; no valid CV command
cv_prog:
	movf	DATA4,W			; value in DATA4
	movwf	MYEEDATA
	movf	TEMP,W
	call	EEPROM_WRITE		; write CV
	goto	acknowledge
cv_verify:
	movf	TEMP,W
	call	EEPROM_READ
test_sm:
	;bsf	STATUS,RP0
	;movf	EEDATA,W
	;bcf	STATUS,RP0

	xorwf	DATA4,W
	btfss	STATUS,Z		; equal than ACK
	goto	exit_bank1		; not equal exit
;---------------------------------------------------------------
acknowledge:
ack_test:
	btfsc	CONFIGa,smflag
	goto	ack_carry_on		; SERVCICE MODE carry on

ack_test_1:
	movf	PWM,F			; PWM = 0 ?
	btfss	STATUS,Z		; if motor stop turn on both outputs
	goto	exit_cv
ack_all:
	bsf	GPIO,gp2		; Both outputs on when motor is off
	nop
	nop
ack_carry_on:
	bsf	GPIO,gp1		; turn output on
	clrf	TEMP1
	movlw	AckTime			; acktime in ms can be set in *.h file
	movwf	TEMP
loop_ack1:
	decfsz	TEMP1,F
	goto	loop_ack1
	decfsz	TEMP,F
	goto	loop_ack1

	movlw	B'00111001'
	andwf	GPIO,F
	;bcf	GPIO,gp1		; turn off
	;nop
	;nop
	;bcf	GPIO,gp2
exit_cv:
	bcf	CONFIGa,smflag		; clear SM mode in case
	bcf	CONFIGa,resetflag
	goto	ReadNMRA
;	goto	exit_bank1
;************* SM Mode *******************************************
					; Service Mode
check_sm:
	btfss	CONFIGa,resetflag	; check for SM, reset packet has to come first
	goto	sm_error

	btfss	CONFIGa,smflag		; Second SM packet
	goto	set_sm_flag

	;switch DATA1 0111 01xx und 0111 11xx -> Direct Mode
	;             0111 00xx und 0111 10xx -> Adress Mode

	btfss	DATA1,2			; Direct Mode, Address Mode
	goto	ad_mode_write
dm_mode_write:
	movf	DATA3,W			; Data in Byte 3
	movwf	DATA4
	movf	DATA2,W			; CV number in Byte 2
	movwf	DATA3
	movf	DATA1,W			; Command in Byte 1
	movwf	DATA2
	goto	DoCV
ad_mode_write:
	movf	DATA2,W			; New Address
	movwf	DATA4
	btfss	DATA1,3
	goto	ad_mode_verify
	movlw	B'00001100'		; Write
	goto	ad_mode_doit
ad_mode_verify:
	movlw	B'00000100'		; Verify
ad_mode_doit
	movwf	DATA2			; Command Verify or Write in DATA2
	clrf	DATA3			; CV1, new address in DATA4
	goto	DoCV
set_sm_flag:
	bsf	CONFIGa,smflag
	goto	exit_bank1
sm_error:
	bcf	CONFIGa,resetflag
	bcf	CONFIGa,smflag
	goto	exit_bank1

;*************DECODING*******************************
decode:
	movf	DATA1,W			; Exclusive or check
	xorwf	DATA2,W
	xorwf	DATA3,W
	xorwf	DATA4,W
	xorwf	DATA5,W
	btfss	STATUS,Z
	goto	exit_bank1		; error in packet

chkconsist:
	bcf	CONFIG1,cst_mode
	movlw	CV19adr
	call	EEPROM_READ
	movwf	TEMP
	andlw	B'01111111'
	iorlw	0
	btfsc	STATUS,Z
	goto	chkadr			; no consist
	bsf	CONFIG1,cst_mode
	bsf	CONFIG1,cst_rev
	btfss	TEMP,7			; reverse mode
	bcf	CONFIG1,cst_rev

	xorwf	DATA1,W
	btfss	STATUS,Z
	goto	chkadr
	bsf	CONFIG1,cst_adr		; valid consist adr
	goto	decode_1

chkadr:
	movlw	.112
	subwf	DATA1,W
	btfss	STATUS,C
	goto	chknormal
	movlw	.128
	subwf	DATA1,W
	btfss	STATUS,C
	goto	chksm
	movlw	B'11000000'
	andwf	DATA1,W
	xorlw	B'11000000'
	btfsc	STATUS,Z
	goto	chk_longadr
	goto	exit_bank1
chknormal:
	movf	DATA1,W			; Set ZERO Flag if so
	btfsc	STATUS,Z
	goto	decode_1		; Valid Broadcast address
;................................
baseline:
	movlw	CV1adr
	call	EEPROM_READ
	xorwf	DATA1,W
	btfss	STATUS,Z
	goto	exit_bank1
	bcf	CONFIG1,cst_adr		; baseline address
	goto	decode_1

;................................
chksm:
;	bcf	DATA1,7			; < 128; check if SM
;	movlw	.112			; DATA1 >= 112, SM Address 112-127
;	subwf	DATA1,W
;	btfss	STATUS,C
;	goto	exit_bank1		; nothing to do for us
	goto	check_sm		; check if valid SM 

chk_longadr:
	btfss	CV29,long_adr
	goto	exit_bank1
	btfsc	DATA1,3	; 128-191
	goto	exit_bank1
	movlw	CV17adr
	call	EEPROM_READ
	xorwf	DATA1,W
	btfss	STATUS,Z
	goto	exit_bank1
	movlw	CV18adr
	call	EEPROM_READ
	xorwf	DATA2,W
	btfss	STATUS,Z
	goto	exit_bank1
	movf	DATA3,W
	movwf	DATA2
	movf	DATA4,W
	movwf	DATA3
;----- Command Decode -------------
decode_1:
	movf	DATA2,W			; Command decoding jump table
	andlw	0xE0			; Isolate command
decode1:
	xorlw	.0			; Consist
	btfsc	STATUS,Z
	goto	ConsistGroup

	bcf	CONFIGa,resetflag
	bcf	CONFIGa,smflag

	xorlw	.64			; Speed reverse
	btfsc	STATUS,Z
	goto	Speed_rev

	xorlw	.32			; Speed forward
	btfsc	STATUS,Z
	goto	Speed_for

	xorlw	.224			; Function one
	btfsc	STATUS,Z
	goto	Function_one

	xorlw	.96			; CV access
	btfsc	STATUS,Z
	goto	CVaccess

	xorlw	.192			; Advanced
	btfsc	STATUS,Z
	goto	exit_bank1		; NOT YET
	
	xorlw	.128			; Function two, not yet
	goto	exit_bank1

;*****************************************************************************************************************
					; Speed command group
Speed_rev:				; Reverse mask
	btfsc	CONFIG1,cst_rev
	goto	Speed_for_cst
Speed_rev_cst:
	bcf	CONFIGa,rev_bit
	btfsc	CV29,norm_rev		; Normal or reverse operation ?
	goto	Sp_for1
Sp_rev1:
	movlw	backward
	movwf	TEMP			; save new dir
	movf	PWM,F
	btfsc	STATUS,Z
	goto	wait_2
	goto	no_wait
wait_2:
	bsf	CONFIG2,FN_ff
	btfsc	CONFIG2,wait_stop
	goto	yes_wait1
	bsf	CONFIG2,wait_stop
	goto	yes_wait2
Speed_for:				; Forward mask
	btfsc	CONFIG1,cst_rev
	goto	Speed_rev_cst
Speed_for_cst:
	bcf	CONFIGa,rev_bit
	btfsc	CV29,norm_rev		; Normal or reverse operation ?
	goto	Sp_rev1
Sp_for1:
	movlw	forward
	movwf	TEMP			; save new dir
	movf	PWM,F
	btfsc	STATUS,Z
	goto	wait_1
	bcf	CONFIG2,wait_stop
	goto	no_wait
wait_1:
	bcf	CONFIG2,FN_ff
	btfsc	CONFIG2,wait_stop
	goto	yes_wait1
	bsf	CONFIG2,wait_stop
yes_wait2:
	movlw	.1
	btfsc	CV64,2			; wait stop if reverse
	movlw	.20
	movwf	WAIT_REV
yes_wait1:
	decfsz	WAIT_REV,f
	goto	yes_wait
	movlw	.1
	movwf	WAIT_REV
	goto	no_wait
yes_wait:
	clrf	DATA2
no_wait:
;************************************************************************************************************
	bcf	CONFIGa,CVaccessBit	; clear CVaccess flag
					; is calc rev bit set, then we came from macro speed
	movf	DATA2,W			; Test emergency
	andlw	0x0F
	xorlw	.1
	btfsc	STATUS,Z
	goto	estop

	btfss	CONFIG1,cst_mode
	goto	new1
	btfss	CONFIG1,cst_adr
	goto	exit_bank1

new1:	btfsc	CONFIGa,calc_rev_bit
	goto	get_saved_speed
					; is actual speed = 0
new2:	movf	PWM,F
	btfsc	STATUS,Z
	goto	calc_speed		; yes, speed is zero
					; now check if new dir = old dir
new3:	movf	MOTMASK,W
	andlw	B'00000110'		; isolate dir bits
	xorwf	TEMP,W			; same dir ?
	btfsc	STATUS,Z		; if not set various bits
	goto	calc_speed		; dir equal than carry on
	movf	DATA2,W			; save new speed
	movwf	NEWSPEED
	andlw	0xF0			; set speed = 0
	movwf	DATA2
	bsf	CONFIGa,rev_bit
	goto	calc_speed_1

get_saved_speed:
	movf	NEWSPEED,W
	movwf	DATA2
	bcf	CONFIGa,calc_rev_bit
;---------------------------------------------------------------
calc_speed:
	movf	MOTMASK,W
	andlw	B'11111001'		; save all other bits set
	iorwf	TEMP,W
	movwf	MOTMASK
calc_speed_1:
	movf	DATA2,W			; Test if Zero = STOP
	andlw	0x0F
	movwf	DATA5
	btfsc	STATUS,Z
	goto	stop			; 0 = STOP
nostop:
	movf	SPEEDSTEP,W		; get value of speedstep
	movwf	TEMP			; into another scratch
	clrw

	bcf	STATUS,C
	rlf	DATA5,F			; multiply by two for speedstep alignment
speedloop:
	addwf	TEMP,W			; Add speedstep and store in W
	decfsz	DATA5,F			; How many times ?
	goto	speedloop
	movwf	SPEEDNEW		; Store in countto variable
	movf	CV2,W			; now add start voltage
	addwf	SPEEDNEW,F

	btfss	CV29,fl_control		; check CV29 bit1 = 1 then intermediate step
	goto	pwm_check

	btfsc	DATA2,4			; do we need intermediate now ?
	goto	pwm_check
	bcf	STATUS,C
	;rrf	TEMP,W			; subtract half-speedstep
	
	movf	TEMP,W
	
	subwf	SPEEDNEW,F

pwm_check:
	bsf	CONFIGa,updown		; set ACC first and check then
	movf	SPEEDNEW,W
	subwf	PWM,W
	btfss	STATUS,C
	goto	checkFL_1
	bcf	CONFIGa,updown		; Speednew < PWM -> DECREMENT
#ifndef testing
	movlw	CV4adr
	call	EEPROM_READ
#endif
#ifdef testing
	movf	CV4,W
#endif
	movwf	CVACCESSRATE
	goto	checkFL
checkFL_1:
#ifndef testing
	movlw	CV3adr
	call	EEPROM_READ
#endif
#ifdef testing
	movf	CV3,W
#endif
	movwf	CVACCESSRATE
	goto	checkFL
estop:
	clrf	PWM
	movlw	B'00111001'
	andwf	GPIO,F
	;bcf	GPIO,gp2
	;nop
	;nop
	;bcf	GPIO,gp1		; motor off with outputs high
stop:
	clrf	SPEEDNEW
	bcf	CONFIGa,updown

	movlw	CV4adr
	call	EEPROM_READ
	movwf	CVACCESSRATE
checkFL:
	btfsc	CV29,fl_control		; if CV29 BIT1=0 then control FL in speed packet
	goto	exit_speed

	btfss	CV29,cv21_bit
	goto	speed_base
	btfss	CONFIG1,cst_adr
	goto	exit_speed
	goto	speed_fl
speed_base:
	btfsc	CONFIG1,cst_adr
	goto	exit_speed
speed_fl:
	btfss	DATA2,4			; FL in DATA byte 2 bit 4
	goto	off_F0F1
	bsf	MOTMASK,FN0		; save for speed macro
	btfss	MOTMASK,forward		; 
	goto	on_F1
	bcf	GPIO,gp0
	nop
	nop
	bsf	GPIO,gp5
	goto	exit_speed
on_F1:
	bsf	GPIO,gp0
	nop
	nop
	bcf	GPIO,gp5
	goto	exit_speed
off_F0F1:
	bcf	MOTMASK,FN0
	movlw	B'00011110'
	andwf	GPIO,F
	;bcf	GPIO,gp0
	;nop
	;nop
	;bcf	GPIO,gp5

exit_speed:
	goto	exit_bank1

;***********************************************************************************************************************
Function_one:				; Function one group
	btfss	CV29,cv21_bit
	goto	chk_base
	btfss	CONFIG1,cst_adr
	goto	exit_bank1
	goto	Function_one1
chk_base:
	btfsc	CONFIG1,cst_adr
	goto	exit_bank1

Function_one1:
	bcf	CONFIGa,CVaccessBit	; clear CVaccess flag
	btfss	CV64,0			; F1 , F0 - change s
	goto	checkF1
	btfss	DATA2,0			; F1 pos 0 at gp5
	goto	carry_on_fn
	btfss	MOTMASK,FN0		; on 	FN0
	goto	lampsim_on_FN0
	btfss	MOTMASK,FN1		; on 	FN1
	goto	lampsim_on_FN1
	goto	exit_bank1
carry_on_fn:
	btfss	CV29,fl_control		; check CV29 bit 1
	goto	exit_bank1		; if zero then Speed packet controls FL
	btfss	DATA2,4			; FL position bit 4 only in the moment
	goto	no_fn
	btfss	CONFIG2,FN_ff
	goto	fn1_on

	btfsc	MOTMASK,FN1		; off 	FN1
	goto	lampsim_of_FN1
	btfss	MOTMASK,FN0		; on 	FN0
	goto	lampsim_on_FN0
	goto	exit_bank1
fn1_on:
	btfsc	MOTMASK,FN0		; off 	FN0
	goto	lampsim_of_FN0
	btfss	MOTMASK,FN1		; on 	FN1
	goto	lampsim_on_FN1
	goto	exit_bank1
no_fn:
	btfsc	MOTMASK,FN0		; off 	FN0
	goto	lampsim_of_FN0
	btfsc	MOTMASK,FN1		; off 	FN1
	goto	lampsim_of_FN1
	goto	exit_bank1

;***********************************************************************************************************************
checkF1:
	btfss	DATA2,0			; F1 pos 0 at gp5
	goto	no_fn1
	btfss	MOTMASK,FN1		; on 	FN1
	goto	lampsim_on_FN1
	goto	carry_on_fna
no_fn1:
	btfsc	MOTMASK,FN1		; off 	FN1
	goto	lampsim_of_FN1
carry_on_fna:	
	btfss	CV29,fl_control		; check CV29 bit 1
	goto	exit_bank1		; if zero then Speed packet controls FL
	btfss	DATA2,4			; FL position bit 4 only in the moment
	goto	no_fn0
	btfss	MOTMASK,FN0		; on 	FN0
	goto	lampsim_on_FN0
	goto	exit_bank1
no_fn0:
	btfsc	MOTMASK,FN0		; off 	FN0
	goto	lampsim_of_FN0
	goto	exit_bank1
;***********************************************************************************************************************
lampsim_on_FN1:
	bsf	CONFIG3,5		; on 	FN1
	goto	lamp_simulate
lampsim_of_FN1:
	bsf	CONFIG3,4		; off 	FN1
	goto	lamp_simulate
lampsim_on_FN0:
	bsf	CONFIG3,7		; on 	FN0
	goto	lamp_simulate
lampsim_of_FN0:
	bsf	CONFIG3,6		; off 	FN0

lamp_simulate:
	btfsc	CONFIG3,7		; on 	FN0
	bsf	MOTMASK,FN0
	btfsc	CONFIG3,6		; off 	FN0
	bcf	MOTMASK,FN0
	btfsc	CONFIG3,5		; on 	FN1
	bsf	MOTMASK,FN1
	btfsc	CONFIG3,4		; off 	FN1
	bcf	MOTMASK,FN1

	movf	PWM,F
	btfss	STATUS,Z		; speed not zero
	goto	exit_bank1

	movlw	.255
	btfss	CV64,1			; lamp simulate on
	movlw	.1			; lamp simulate off
	movwf	lamp_sim
lamploop:
	btfsc	CONFIG3,7		; on 	FN0
	bcf	GPIO,gp0
	btfsc	CONFIG3,6		; off 	FN0
	bsf	GPIO,gp0
	btfsc	CONFIG3,5		; on 	FN1
	bcf	GPIO,gp5
	btfsc	CONFIG3,4		; off 	FN1
	bsf	GPIO,gp5
	movf	lamp_sim,W
	call	wait_lamp

	btfsc	CONFIG3,7
	bsf	GPIO,gp0
	btfsc	CONFIG3,6
	bcf	GPIO,gp0
	btfsc	CONFIG3,5
	bsf	GPIO,gp5
	btfsc	CONFIG3,4
	bcf	GPIO,gp5
	movf	lamp_sim,W
	sublw	.1
	call	wait_lamp

	decfsz	lamp_sim,F			
	goto	lamploop
	goto	exit_bank1
;---------------------------------------
wait_lamp:
	movwf	lamp_temp
lamp_1:
	nop
	nop
	nop
	nop
	nop
	nop
	decfsz	lamp_temp,F		; loop ON	
	goto	lamp_1
	return

;***********************************************************************************************************************
					; Consist Group
ConsistGroup:
	bcf	CONFIGa,CVaccessBit	; clear CVaccess flag
	movf	DATA2,W
	xorlw	.0
	goto	soft_reset
	xorlw	.1
	goto	hard_reset
	andlw	B'00010010'
	xorlw	B'00010010'
	btfsc	STATUS,Z
	goto	cst_ctrl_set
	goto	exit_consist
hard_reset:
	clrf	MYEEDATA		; no consist
	movlw	CV19adr			; get EEPROM location of CV19
	call	EEPROM_WRITE

	movlw	cv29default		; CV29	
	movwf	MYEEDATA
	movlw	CV29adr			; get EEPROM location of CV29
	call	EEPROM_WRITE

soft_reset:
	bsf	CONFIGa,resetflag	; set resetflag for SM mode detection
	clrf	PWM
	clrf	SPEEDNEW
	
	movlw	B'00111001'
	andwf	GPIO,F
	;bcf	GPIO,gp1		; turn off motor
	;nop
	;nop
	;bcf	GPIO,gp2
	goto	exit_consist

cst_ctrl_set:
	bsf		DATA3,7
	btfss	DATA2,0
	bcf		DATA3,7
	movf	DATA3,W
	movwf	MYEEDATA
	movlw	CV19adr
	call	EEPROM_WRITE
exit_consist:
	goto	exit_bank1
;***********************************************************************************************************************
exit_bank1:
	clrf	CONFIG1
	clrf	CONFIG3
	clrf	TIMEOUTCOUNT		; ANALOG new !
	clrf	TEMP
	clrf	DATA1
	clrf	DATA2
	clrf	DATA3
	clrf	DATA4
	clrf	DATA5
	clrf	ENDVAL
	return
;***********************************************************************************************************************
ChkPowerUp:
	movlw	0x7F			; last EEPROM cell
	call	EEPROM_READ
	xorlw	0xaa
	btfsc	STATUS,Z
	return			 	; Already Powered Up

	movlw	.3
	movwf	MYEEDATA
	movlw	CV1adr			; CV1=3
	call	EEPROM_WRITE
	movlw	CV64adr			; CV64=3
	call	EEPROM_WRITE
	movlw	CV9adr			; CV9=3
	call	EEPROM_WRITE
	movlw	.2			
	movwf	MYEEDATA
	movlw	CV29adr			; CV29=2
	call	EEPROM_WRITE
	clrf	MYEEDATA		
	movlw	CV19adr			; CV19=0
	call	EEPROM_WRITE
	movlw	.1
	movwf	MYEEDATA
	movlw	CV3adr			; CV3=1
	call	EEPROM_WRITE
	movlw	CV4adr			; CV4=2
	call	EEPROM_WRITE
	movlw	.20			
	movwf	MYEEDATA
	movlw	CV2adr			; CV2=20 PWM Min
	call	EEPROM_WRITE
	movlw	CV65adr			; CV65=20 KickStart
	call	EEPROM_WRITE
	movlw	.200			
	movwf	MYEEDATA
	movlw	CV5adr			; CV5=200 PWM Max
	call	EEPROM_WRITE

	movlw	0xaa
	movwf	MYEEDATA
	movlw	0x7F		 	; stamp power up
	call	EEPROM_WRITE
	return

	END