LIST    P=16F88,R=HEX
;
;       LCD TACHOMETER FOR R/C MODEL ENGINE
;       SUN, PU-NENG
;
;	VERSION 4.35: 12/02/2005 AUTO NEUTRAL FOR ALL DST
;	VERSION 4.32: 11/12/2004 FIX LCD DRIVER, ADD BONUS 
;	VERSION 4.2: 10/02/2004 16F88 ADC FOR ANALOG INPUT,
;				PITCH/AIR SPEED	
;	VERSION 4.1: 09/26/2004 16*2 LCD GRAPHIC MODE 16F628, 
;				 HIGH RESOLUTION TACHOMETER
;       VERSION 4.0: 09/18/2004  16*2 LCD GRAPHIC MODE 16F628
;	VERSION 3.3: 09/11/2004  16*2 LCD	 	
;	VERSION 3.2: 09/08/2004  RA,4 CONTROLL +5V OUTPUT
;     	VERSION 3.1: 09/04/2004  LEAVES SELECT
;	VERSION 3.0: 08/30/2004	 ADD REC_T V1 FOR RECEIVER TESTING
;	VERSION 2.0: 08/26/2004  ADD MID & RANDOM MODES
;       VERSION 1.0: 08/25/2004
;       
;-------SYSTEM---------------------    
#INCLUDE <P16F88.INC>
#INCLUDE <MATH16.INC>   
;Program Configuration Register 1
		__CONFIG    _CONFIG1, _CP_ALL & _CCP1_RB3 & _DEBUG_OFF & _WRT_PROTECT_OFF & _CPD_OFF & _LVP_OFF & _BODEN_OFF & _MCLR_OFF & _PWRTE_ON & _WDT_OFF & _XT_OSC

;Program Configuration Register 2
		__CONFIG    _CONFIG2, _IESO_OFF & _FCMEN_OFF

;-------------------------------------------------                   
PC      EQU     H'0002'
RA      EQU     H'0005'
RB      EQU     H'0006'
OPTIONF EQU     H'0081'
;-------DEFINE BIT-------------
;
C	EQU 0           
W       EQU 0           ;d VALUE FOR INCF......
F       EQU 1           ;d VALUE
OFF     EQU 0           ;FLAG BIT 0,
RP0     EQU 5           ;STATUS BIT 5 & 6 =REGISTER BANK SELECT
RP1     EQU 6          
INPUT   EQU 7           ;RB7 AS INPUT PORT
TOIF    EQU 2           ;INTCON TMR0 OVERLOW INT FLAG
TOIE    EQU 5           ;INTCON TOIF ENABLE
GIE     EQU 7           ;INTCON GLOBAL INT ENABLE
	

;-------USER-------------------------------------
;-------REGISTERS------
;
EE1     EQU H'20'       
EE2	EQU H'21'
EE3     EQU H'22'
EE4     EQU H'23'        
FF1	EQU H'24'
FF2     EQU H'25'
FF3     EQU H'26'      
FF4	EQU H'27'
P_SEL	EQU H'28'
P_HIGH	EQU H'29'
P_LOW	EQU H'2A'
	
TCNT0   EQU H'2C'        ;= (0.6SEC /INT* PRESCAL*CLOCK)
INTR0   EQU H'2D'        ;AFTER INT, SAVE W VALUE 
INTR1   EQU H'2E'        ;AFTER INT, SAVE STATUS 
DD1     EQU H'2F'        ;DATA FOR INPUT RPM
DD2	EQU H'30'
DD3     EQU H'31'
TEMP1   EQU H'32'       ;
TEMP2   EQU H'33'
TEMP3   EQU H'34'
GG1	EQU H'2B'
GG2	EQU H'35'		;100-200 +- 1 EVERY INT_DST
GG3	EQU H'4E'	
GG4	EQU H'4F'	
INT_C	EQU H'36'	;0-255 -1 EVERY INT_DST
FLAG_REG	EQU H'37'
MODE_SEL	EQU H'38'
PWM_C	EQU H'39'	
PUSH_C	EQU H'3A'
RAND_HI	EQU H'3B'
RAND_LO EQU H'3C'
TCNT	EQU H'3D'
TEMP4   EQU H'3E'
TEMP5   EQU H'3F'
;-------BIT----------------
F_ON	EQU 0
F_UP	EQU 1
F_TDST	EQU 2
F_INT	EQU 3
F_RECT	EQU 6
F_SCT	EQU 7
TACHO_SEL	EQU 4
ESCP	EQU 5
;
;-------VARIABLES-----------
;
;TMR     EQU D'181'      ;(PRESCALE 64*1uS) * 75 = 4800 uS
;TCNT_2    EQU D'125'    ;4.8mS*125=600mS,BUT FIX SOME ODD CODE  
;TCNT_3    EQU D'84'  	;THIS TWO * PRESCALE 1:64,TO A 0.6 SECOND DURATION 
;TCNT_4    EQU D'62'  
;TCNT_5    EQU D'50'  
BDELAY    EQU D'50'  	;BUTTON DELAY
LDELAY    EQU D'100'  	;DISPLAY DELAY

TMR1    EQU D'178'      ;(PRESCALE 256*78) = 19968 uS

RLIMIT	EQU D'200'	;SERVO LIMIT
LLIMIT	EQU D'100'

;-------LCD SOUTINE-------------------------
;-------REGISTERS---------
;
ADDRESS1	EQU H'41'
CHAR1		EQU H'42'
TEMP1_LCD	EQU H'43'
TEMP2_LCD	EQU H'44'
TEMP3_LCD	EQU H'45'
TEMP4_LCD	EQU H'46'
TABLE_INDEX	EQU H'47'	
TEMPH		EQU H'48'
TEMPL		EQU H'49'
COUNT		EQU H'4A'
R0		EQU H'4B'
R1		EQU H'4C'
R2		EQU H'4D'	;
;
;
;==========   MAIN PROGRAM   =========
;
        ORG     0
        GOTO    START
;----------------------------------------	
;
	ORG	4		;INTERRUPT BEGIN

INTB    MOVWF   INTR0           ; SAVE W 
        MOVF    STATUS,W        ; SAVE STATUS
        MOVWF   INTR1

	BTFSC	FLAG_REG,F_RECT	
	GOTO 	INT_RECT
	
	MOVLW   TMR1            ;256*78=19968 uS
	MOVWF	TMR0
	CALL	PRINT_PWM
;---------------------------------------------	
	BSF	RB,1
INT_DST1			;10 uS LOOP 
	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
	NOP
	DECFSZ	TEMP5,F		;TEMP5
	GOTO	INT_DST1	;END OF LOOP
	BCF	RB,1
	
	
INT_DST2	
	BTFSS	FLAG_REG,F_SCT
	GOTO	INTEND	
	CALL	SET_AN2
	CALL	START_ADC
	MOVLW	0
	CALL	PRINT_VOLT	
	GOTO	INTEND
INT_RECT
	CLRF	TMR0

INTEND
	BSF	FLAG_REG,F_INT	
	DECF	INT_C,F		;DECREASE INT_COUNTER
	MOVF    INTR1,W         ; RESTORE STATUS
        MOVWF   STATUS
        MOVF    INTR0,W         ; RESTORE W
        BCF     INTCON,TOIF     ; RESET FLAG OF INT T0
        RETFIE
;   
;================================================
;------ END OF TACHO INT ---------
;================================================
PRINT_PWM
	GOTO	PRINT_PWM2
PRINT_PWM3
	return
;-------------------------------------------



;------ THE MATH SUB ARE IN MOMERY PAGE 1 ------

FXD3216L	
	BCF	PCLATH,4
	BSF	PCLATH,3
	GOTO	FXD3216L2
FXD3216L3        
	RETURN
;------------------------------
FXM1616U
	BCF	PCLATH,4
	BSF	PCLATH,3
	GOTO 	FXM1616U2
FXM1616U3
	RETURN
;--------------------------------
PRINT_RPM
	GOTO 	PRINT_RPM2
PRINT_RPM3
	return
;--------------------------------
PRINT_VOLT
	GOTO 	PRINT_VOLT2
PRINT_VOLT3
	RETURN
;--------------------------------
PRINT_KMH
	GOTO 	PRINT_KMH2
PRINT_KMH3
	RETURN		
;---------------------------------
BIN2BCD
	GOTO 	BIN2BCD2
BIN2BCD3
	return
;-------------------------------
BIN2BCD16
	GOTO	BIN2BCD162
BIN2BCD163
	return
;-----------------------------
BONUS_WARN
	GOTO 	BONUS_WARN2
BONUS_WARN3
	return
;------------------------------
SET_AN2
	BCF     STATUS,RP1
	BSF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
	MOVLW	B'11000000'
	MOVWF	ADCON1		;RIGHT JUSTIFIED ADCS2
	BCF     STATUS,RP0      ;ENABLE BANK0
	MOVLW	B'01010000'
	MOVWF	ADCON0		;T/16 AN2
	return
;-------------------------------
SET_AN3
	BCF     STATUS,RP1
	BSF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
	MOVLW	B'01000000'
	MOVWF	ADCON1		;LEFT JUSTIFIED ADCS2
	BCF     STATUS,RP0      ;ENABLE BANK0
	MOVLW	B'01011000'
	MOVWF	ADCON0		;T/16 AN3
	return
;------------------------------------------
START_ADC
	BSF	ADCON0,ADON	;ADC ON
	CALL 	LOOP_S
	BSF	ADCON0,GO	;START ADC
START_ADC1
	BTFSC	ADCON0,GO		
	GOTO 	START_ADC1
	BCF	ADCON0,ADON	;;ADC OFF
	return
;=========================================






  
;=========================================
;LCD SOUTINE
;=========================================
CGRAM_ADDRESS
        MOVWF   ADDRESS1
	BSF	ADDRESS1,6	;CGRAM
	MOVF	ADDRESS1,0
        ANDLW   B'11110000'     ;    
        CALL    PRINT_CONT
        SWAPF   ADDRESS1,W	;SWAP HALVES F
        ANDLW   B'11110000'     ;       
        CALL    PRINT_CONT
	RETURN

PRINT_DATA
	BSF	RB,2		;RS=1, DATA     
	MOVWF   ADDRESS1
	BSF	ADDRESS1,2	
	MOVF	ADDRESS1,0
	MOVWF   RB 
	BSF     RB,3    
	NOP             
        BCF     RB,3                 
        CALL    LOOP_S		;100 uS
        RETURN

PRINT_CONT
	MOVWF   RB      
	BSF     RB,3    
	NOP             
        BCF     RB,3                 
        CALL    LOOP_S		;100 uS
        RETURN

CLEAR_DISPLAY
        MOVLW   B'00000000'	;0 
        CALL	PRINT_CONT                 
        MOVLW   B'00010000'	;1 CLEAR 
        CALL	PRINT_CONT
	MOVLW   1
        CALL    LOOP_L		;10mS                         
        RETURN

DDRAM_ADDRESS
        MOVWF   ADDRESS1
	BSF	ADDRESS1,7	;DDRAM
	MOVF	ADDRESS1,0
        ANDLW   B'11110000'     ;    
        CALL    PRINT_CONT
        SWAPF   ADDRESS1,W	;SWAP HALVES F
        ANDLW   B'11110000'     ;       
        CALL    PRINT_CONT
	RETURN

PRINT_CHAR
        MOVWF   CHAR1
   	ANDLW   B'11110000'     ;       
        CALL    PRINT_DATA
        SWAPF   CHAR1,W		;SWAP HALVES F
       	ANDLW   B'11110000'     ;SENT LOW 4 BITS      
        CALL    PRINT_DATA
        RETURN

LOOP_S				;120uS LOOP 
        MOVLW	D'40'
	MOVWF   TEMP4_LCD
LOOP_S1
        DECFSZ  TEMP4_LCD,1	;
        GOTO    LOOP_S1
	RETURN

LOOP_L				;10 mS LOOP
	MOVWF   TEMP1_LCD	
LOOP_L1 
	MOVLW	D'10'		;10 mS
	MOVWF   TEMP2_LCD
LOOP_L2	
	MOVLW	D'249'		;(249*4)+5=1001 
	MOVWF   TEMP3_LCD
LOOP_L3
        NOP
	DECFSZ  TEMP3_LCD,1
        GOTO    LOOP_L3
	DECFSZ    TEMP2_LCD,1
        GOTO    LOOP_L2
	DECFSZ    TEMP1_LCD,1
        GOTO	LOOP_L1
	RETURN

DISP_MESSAGE
	MOVWF   TABLE_INDEX	;W -> INDEX, THE BEGIN OF MESSAGE
        CALL    MESSAGE1                 
        ANDLW   0FFH
        SKPNZ        		; IF TABLE RETURN 0 , END OF MESSAGE SECTION       
        RETURN                         
        CALL    PRINT_CHAR	; DOSPLAY
  	INCF   	TABLE_INDEX,0	
	GOTO    DISP_MESSAGE
;====================================================
MODE_ADDRESS
                            
        ADDWF   PC,F
        RETLW   0
	RETLW   0
        RETLW   D'16'
        RETLW   D'31'
        RETLW   D'46'
            
MESSAGE1                        
                             
        ADDWF   PC,F
        
	RETLW   ' '
        RETLW   'T'
        RETLW   'a'
        RETLW   'c'
        RETLW   'h'	
        RETLW   'o'
        RETLW   'm'
        RETLW   'e'
        RETLW   't'
        RETLW   'e'
	RETLW   'r'
	RETLW   ' '
	RETLW   ' '
	RETLW   ' '
	RETLW   ' '
        RETLW   0
	RETLW   ' '
	RETLW   'D'	;16
        RETLW   'i'
        RETLW   'g'
        RETLW   'i'
        RETLW   ' '
	RETLW   'S'
        RETLW   'e'
	RETLW   'r'	
        RETLW   'v'
        RETLW   'o'
        RETLW   ' '
	RETLW   'T'
        RETLW   'x'	
	
        RETLW   0
	RETLW   ' '
        RETLW   'S'	;31
        RETLW   'p'
        RETLW   'e'
        RETLW   'e'
	RETLW   'd'
        RETLW   ' '
	RETLW   'C'	
        RETLW   't'
        RETLW   'r'
        RETLW   'l'
	RETLW   ' '
	RETLW   'T'
        RETLW   'x'	
        RETLW   0
        RETLW   ' '
        RETLW   'R'	;46
        RETLW   'e'
	RETLW   'c'
        RETLW   '_'	
        RETLW   'T'
        RETLW   'r'
	RETLW   'a'
	RETLW   'n'	
        RETLW   's'
	RETLW   ' '
	RETLW   'T'	
        RETLW   'x'
	RETLW   ' '
	
	RETLW   0
	
	NOP		
	NOP
	NOP
	NOP
	

;=======================================
;---------- PROGRAM BEGIN -----------
;----------------------------------------
;
	
START
	CLRF 	RA		;16F88 ONLY
	
	BCF     STATUS,RP1
	BSF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
	MOVLW	B'00001100'
	MOVWF	ANSEL		;ANSEL ANS2,3
        MOVLW 	B'00000001'
        MOVWF 	TRISB		;SET RB 1-7 OUTPUT, 0 INPUT 
        MOVLW 	B'00101111'
        MOVWF 	TRISA     	;SET RA0-RA2 INTPUT, RA3 ANALOG,RA4 OUTPUT
	BCF     STATUS,RP0      ;ENABLE BANK0

	
	BCF	RB,1		;OUT PWM
	BSF	RA,4		;OUT +5V OFF
	CLRF 	RB
	CLRF	FLAG_REG
        BCF	INTCON,GIE

INIT_LCD
	MOVLW	5		;POWER ON DELAY 50mS
	CALL 	LOOP_L	
SET_4BIT_MOD			;SET LCD AT 4 BIT MODEL
       	MOVLW   B'00110000'	;3
	CALL	PRINT_CONT
        MOVLW   1
        CALL    LOOP_L		;10mS        
        MOVLW   B'00110000'	;3
        CALL	PRINT_CONT
        MOVLW   B'00110000'	;3
	CALL	PRINT_CONT
        MOVLW   B'00100000'	;2, 4BIT MODE
        CALL	PRINT_CONT                 
        MOVLW   B'00100000'	;2
        CALL	PRINT_CONT                
        MOVLW   B'10000000'	;8 4BITS, 2 LINES, FONT 0
        CALL	PRINT_CONT                   
             
INIT_DISPLAY			
        MOVLW   B'00000000'	;O
        CALL	PRINT_CONT                   
        MOVLW   B'11000000'	;C SCREEN ON, NO CURSOR, NO BLINKING
        CALL	PRINT_CONT     
	CALL	CLEAR_DISPLAY	;CLEAR DISPLAY
	
;------ BONUS ------------------
	BTFSC	RA,0
	GOTO 	MAIN_MAKE
	BTFSS	RA,5
	
	GOTO	BONUS
MAIN_MAKE	
	GOTO 	MAKEWORD
MAIN_M
	MOVLW	H'40'
	CALL	DDRAM_ADDRESS
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	'M'		
	CALL    PRINT_CHAR	
	MOVLW 	'T'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	'V'		
	CALL    PRINT_CHAR	
	MOVLW 	'4'		
	CALL    PRINT_CHAR	
	MOVLW 	'.'		
	CALL    PRINT_CHAR	
	MOVLW 	'3'		
	CALL    PRINT_CHAR	
	MOVLW 	'5'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	
ESCMODE
	MOVLW	H'0'
	CALL	DDRAM_ADDRESS
	
	MOVLW 	'i'		
	CALL    PRINT_CHAR	
	MOVLW 	'L'		;DISPLAY FIRST SECTION MESSAGE
	CALL    PRINT_CHAR	
	MOVLW 	'u'		
	CALL    PRINT_CHAR	
	MOVLW 	'F'		
	CALL    PRINT_CHAR	
	MOVLW 	'a'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	'H'		
	CALL    PRINT_CHAR	
	MOVLW 	'o'		
	CALL    PRINT_CHAR	
	MOVLW 	'b'		
	CALL    PRINT_CHAR	
	MOVLW 	'b'		
	CALL    PRINT_CHAR	
	MOVLW 	'y'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	'W'		
	CALL    PRINT_CHAR	
	MOVLW 	'e'		
	CALL    PRINT_CHAR	
	MOVLW 	'b'		
	CALL    PRINT_CHAR
	
	MOVLW	LDELAY	
	CALL	LOOP_L		;DELAY 2.5 SECOND
	CLRF	MODE_SEL
	INCF	MODE_SEL,F
	
;------------------------------------------
MMODE_00
	BCF	INTCON,GIE
	BCF	INTCON,TOIE
	BCF	FLAG_REG,ESCP
	MOVLW	H'40'
	CALL	DDRAM_ADDRESS
	MOVFW	MODE_SEL
	CALL	MODE_ADDRESS	;GET MODE NAME BEGINING
	CALL 	DISP_MESSAGE	;DISPLAY MODE NAME
	MOVLW	BDELAY
	CALL 	LOOP_L		;
MMODE_99
	BTFSS	RA,1
	GOTO	MMODE_01	; 
	BTFSS	RA,0
	GOTO	MMODE_02	;SELECT PUSH, 
	GOTO	MMODE_99
MMODE_01
	INCF	MODE_SEL,F
	MOVLW	5		;MODE_SEL 1-4 ONLY
	SUBWF	MODE_SEL,W
	SKPZ
	GOTO	MMODE_00
	MOVLW	1
	MOVWF	MODE_SEL
	GOTO	MMODE_00
MMODE_02
	
;-------ON MODE_SEL GOTO----------
	MOVLW	1		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	MAIN		;1 TACHO
	MOVLW	2		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	DST_MAIN	;2 DST
	MOVLW	3		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	SCT_MAIN	;3 SCT
	MOVLW	4		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	RECT_MAIN	;4 RECT
;------------------------------------









;-------------------------------------------
MAIN  
	BSF	RA,4		;OUT +5V OFF
	BCF	INTCON,GIE
	CALL	CLEAR_DISPLAY	;CLEAR DISPLAY

	MOVLW	BDELAY		;DELAY............
	CALL 	LOOP_L		
	BCF	FLAG_REG,TACHO_SEL
	MOVLW 	1		;DEFAULT 2 LEAVES
	MOVWF 	MODE_SEL
	
;-----  INIT RPM & KM/h DISPLAY   --------
	MOVLW	D'20'		;
	MOVWF	P_SEL
	

	CLRF	TEMPH	
	CLRF	TEMPL	
	CALL 	BIN2BCD16
	CALL	PRINT_RPM
	CALL	PRINT_KMH	
	
;------ TACHO MODE SELECT -----------
T_SEL	
	INCF 	MODE_SEL,F
	MOVLW	6		;MODE_SEL BLADE 1-5 
	SUBWF	MODE_SEL,W
	SKPZ
	GOTO	T_SEL1
	MOVLW	1
	MOVWF	MODE_SEL
T_SEL1	
	MOVLW	H'00'
	CALL	DDRAM_ADDRESS
	MOVLW	H'30'		;CONVERT HEX TO ASCII
	ADDWF	MODE_SEL,W	;CONVERT HEX TO ASCII
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	'B'		
	CALL    PRINT_CHAR	
	MOVLW 	's'		
	CALL    PRINT_CHAR	

;-------ON MODE_SEL GOTO----------
	MOVLW	1		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	TMODE_1		;1 BLADE
	MOVLW	2		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	TMODE_2		;2 BLADES
	MOVLW	3		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	TMODE_3		;3
	MOVLW	4		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	TMODE_4		;4
;------------------------------------
TMODE_5				;5 
	MOVLW 	H'0'
	MOVWF	GG1
	MOVLW 	H'16'
	MOVWF	GG2
	MOVLW 	H'E3'
	MOVWF	GG3
	MOVLW 	H'60'
	MOVWF	GG4
	
	MOVLW 	H'0'
	MOVWF	EE1
	MOVLW 	H'B7'
	MOVWF	EE2
	MOVLW 	H'1B'
	MOVWF	EE3
	MOVLW 	H'00'
	MOVWF	EE4

	MOVLW 	H'0E'
	MOVWF	FF1
	MOVLW 	H'4E'
	MOVWF	FF2
	MOVLW 	H'1C'
	MOVWF	FF3
	MOVLW 	H'00'
	MOVWF	FF4
	GOTO	T_SEL_5
TMODE_4		
	MOVLW 	H'0'
	MOVWF	GG1
	MOVLW 	H'1C'
	MOVWF	GG2
	MOVLW 	H'9C'
	MOVWF	GG3
	MOVLW 	H'38'
	MOVWF	GG4
	
	MOVLW 	H'0'
	MOVWF	EE1
	MOVLW 	H'E4'
	MOVWF	EE2
	MOVLW 	H'E1'
	MOVWF	EE3
	MOVLW 	H'C0'
	MOVWF	EE4

	MOVLW 	H'11'
	MOVWF	FF1
	MOVLW 	H'E1'
	MOVWF	FF2
	MOVLW 	H'A3'
	MOVWF	FF3
	MOVLW 	H'00'
	MOVWF	FF4
	GOTO	T_SEL_5
TMODE_3		
	MOVLW 	H'0'
	MOVWF	GG1
	MOVLW 	H'26'
	MOVWF	GG2
	MOVLW 	H'25'
	MOVWF	GG3
	MOVLW 	H'A0'
	MOVWF	GG4
	
	MOVLW 	H'01'
	MOVWF	EE1
	MOVLW 	H'31'
	MOVWF	EE2
	MOVLW 	H'2D'
	MOVWF	EE3
	MOVLW 	H'00'
	MOVWF	EE4

	MOVLW 	H'17'
	MOVWF	FF1
	MOVLW 	H'B7'
	MOVWF	FF2
	MOVLW 	H'84'
	MOVWF	FF3
	MOVLW 	H'00'
	MOVWF	FF4
	GOTO	T_SEL_5
TMODE_2		
	MOVLW 	H'0'
	MOVWF	GG1
	MOVLW 	H'39'
	MOVWF	GG2
	MOVLW 	H'38'
	MOVWF	GG3
	MOVLW 	H'70'
	MOVWF	GG4
	
	MOVLW 	H'01'
	MOVWF	EE1
	MOVLW 	H'C9'
	MOVWF	EE2
	MOVLW 	H'C3'
	MOVWF	EE3
	MOVLW 	H'80'
	MOVWF	EE4

	MOVLW 	H'23'
	MOVWF	FF1
	MOVLW 	H'C3'
	MOVWF	FF2
	MOVLW 	H'46'
	MOVWF	FF3
	MOVLW 	H'00'
	MOVWF	FF4
	GOTO	T_SEL_5	
TMODE_1		
	MOVLW 	H'0'
	MOVWF	GG1
	MOVLW 	H'72'
	MOVWF	GG2
	MOVLW 	H'70'
	MOVWF	GG3
	MOVLW 	H'E0'
	MOVWF	GG4
	
	MOVLW 	H'03'
	MOVWF	EE1
	MOVLW 	H'93'
	MOVWF	EE2
	MOVLW 	H'87'
	MOVWF	EE3
	MOVLW 	H'00'
	MOVWF	EE4

	MOVLW 	H'47'
	MOVWF	FF1
	MOVLW 	H'86'
	MOVWF	FF2
	MOVLW 	H'8C'
	MOVWF	FF3
	MOVLW 	H'00'
	MOVWF	FF4
	GOTO	T_SEL_5	
T_SEL_5

	GOTO    P_SEL2


;------------------------------------
;------ TACHO PITCH SELECT -----------

	
P_SEL1	
	MOVLW  	D'5'
	ADDWF	P_SEL,F
	MOVLW	D'100'		;MODE_SEL 2-5 ONLY
	SUBWF	P_SEL,W
	SKPZ
	GOTO	P_SEL2
	MOVLW	D'20'
	MOVWF	P_SEL
P_SEL2
	MOVFW	P_SEL
	CALL 	BIN2BCD
	
	MOVLW	H'40'
	CALL	DDRAM_ADDRESS
	MOVLW	H'30'		;CONVERT HEX TO ASCII
	ADDWF	DD2,W		;CONVERT HEX TO ASCII
	CALL    PRINT_CHAR	
	MOVLW 	'.'		
	CALL    PRINT_CHAR	
	MOVLW	H'30'		;CONVERT HEX TO ASCII
	ADDWF	DD3,W		;CONVERT HEX TO ASCII
	CALL    PRINT_CHAR	
	MOVLW 	'"'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	
	MOVLW	BDELAY		;DELAY............
	CALL 	LOOP_L


;-----  CAL P_HIGH, P_LOW FROM P_SEL  ------
;
;	CALL  FXM1616U
;       Result: AARGB 0~3  <--  AARGB 0~1 x BARGB 0~1
;
;       Use:    CALL    FXD3216L
;       Result: AARG 0~3 , REM  <--  AARG / BARG 0~1
	CLRF  	AARGB0
	MOVFW	P_SEL
	MOVWF	AARGB1
	MOVLW	H'5'
	MOVWF	BARGB0	
	MOVLW	H'F4'
	MOVWF	BARGB1
	CALL	FXM1616U	;P_SEL * 1524
	MOVLW	H'0'
	MOVWF	BARGB0	
	MOVLW	H'62'
	MOVWF	BARGB1	
	CALL	FXD3216L	;/98
	MOVFW	AARGB2	
	MOVWF	P_HIGH
	MOVFW	AARGB3	
	MOVWF	P_LOW
	
	GOTO	MAIN_0


;---------------------------------
MAIN_0  
		
	MOVLW	B'00000000'
	MOVWF	T1CON		;T1 TIMER MODE
	
	BSF     STATUS,RP0  
	BSF	PIE1,TMR1IE
	BCF     STATUS,RP0  
	BSF	INTCON,GIE	;ENABLE INT
	BCF	INTCON,PEIE	;DISABLE INT
	CLRF	TMR1H
	CLRF	TMR1L
MAIN10
	CLRF	AARGB2
	CLRF	AARGB3	
;-----------------------------------

		
;----------------------------------

MAIN01	
	MOVF	AARGB2,W
	MOVWF	AARGB0
	MOVWF	TEMPH	
	MOVF	AARGB3,W
	MOVWF	AARGB1
	MOVWF	TEMPL	
	CALL 	BIN2BCD16
	CALL	PRINT_RPM	

	MOVFW 	P_HIGH
	MOVWF	BARGB0
	MOVFW 	P_LOW
	MOVWF	BARGB1
	
	CALL  	FXM1616U
	BCF	STATUS,C
	RRF	AARGB0,F
	RRF	AARGB1,F
	RRF	AARGB2,F
	BCF	STATUS,C
	RRF	AARGB0,F
	RRF	AARGB1,F
	RRF	AARGB2,F

	MOVFW	AARGB1
	MOVWF	TEMPH	
	MOVFW	AARGB2
	MOVWF	TEMPL	
	CALL 	BIN2BCD16
	CALL	PRINT_KMH	

	MOVLW	D'30'	
	CALL	LOOP_L		
MAIN11
	BSF	T1CON,T1CKPS0
	BSF	T1CON,T1CKPS1	;PRESCALE 8
	CLRF	TMR1H
	CLRF	TMR1L
	BCF	PIR1,TMR1IF
MAIN02
	
	BTFSC	PORTB,0
	GOTO 	MAIN02
	BSF	T1CON,TMR1ON	;START
MAIN03
	BTFSC	PIR1,TMR1IF
	GOTO	MAIN10		;OVER FLOW
	BTFSS	PORTA,0
	GOTO	P_SEL1
	BTFSS	PORTA,1
	GOTO	T_SEL
	BTFSS	RA,5
	GOTO	START
	BTFSS	PORTB,0
	GOTO 	MAIN03
	BCF	T1CON,TMR1ON	;STOP
	BCF	PIR1,TMR1IF
	CLRF	TMR1H
	CLRF	TMR1L
	BSF	T1CON,TMR1ON	;START AGAIN
MAIN04
	
	BTFSC	PORTB,0
	GOTO 	MAIN04
MAIN05
	BTFSC	PIR1,TMR1IF
	GOTO	MAIN10
;	BTFSS	PORTA,0
;	GOTO	P_SEL1	
;	BTFSS	PORTA,1
;	GOTO	T_SEL
;	BTFSS	RA,5
;	GOTO	START
	BTFSS	PORTB,0
	GOTO 	MAIN05
	BCF	T1CON,TMR1ON	;STOP
	BTFSC	PIR1,TMR1IF
	GOTO	MAIN10		;OVER FLOW
MAIN07
	MOVLW	D'31'
	SUBWF	TMR1H,W		
	SKPNC			;F<W , C=0
	GOTO	MAIN08		;1/8 IS OK
	MOVLW	D'3'
	SUBWF	TMR1H,W		
	SKPNC			;F<W , C=0
	GOTO	NATURE11		;2/1
	GOTO	DIVI80		;20/1
MAIN08	
	MOVFW 	GG1
	MOVWF	AARGB0
	MOVFW 	GG2
	MOVWF	AARGB1
	MOVFW 	GG3
	MOVWF	AARGB2
	MOVFW 	GG4
	MOVWF	AARGB3
	MOVF 	TMR1H,W
	MOVWF	BARGB0
	MOVF 	TMR1L,W
  	MOVWF	BARGB1	
        
        CALL    FXD3216L
	
	
	GOTO	MAIN01
;------------------------------------------
DIVI80
	BCF	T1CON,T1CKPS0
	BCF	T1CON,T1CKPS1	;PRESCALE 1
DIVI01	
	CLRF	TMR1H
	CLRF	TMR1L
	BCF	PIR1,TMR1IF
	MOVLW	D'20'
	MOVWF	INT_C
DIVI02
	
	BTFSC	PORTB,0
	GOTO 	DIVI02
	BSF	T1CON,TMR1ON	;START
DIVI03
	BTFSC	PIR1,TMR1IF
	GOTO	NATURE11	;OVER FLOW
	BTFSS	PORTA,0
	GOTO	P_SEL1	
	BTFSS	PORTA,1
	GOTO	T_SEL
	BTFSS	RA,5
	GOTO	START
	BTFSS	PORTB,0
	GOTO 	DIVI03
	BCF	T1CON,TMR1ON	;STOP
	BCF	PIR1,TMR1IF
	CLRF	TMR1H
	CLRF	TMR1L
	BSF	T1CON,TMR1ON	;START
DIVI04
	
	BTFSC	PORTB,0
	GOTO 	DIVI04
DIVI05
	BTFSC	PIR1,TMR1IF
	GOTO	NATURE11		;OVER FLOW
;	BTFSS	PORTA,0
;	GOTO	P_SEL1	
;	BTFSS	PORTA,1
;	GOTO	T_SEL
;	BTFSS	RA,5
;	GOTO	START
	BTFSS	PORTB,0
	GOTO 	DIVI05
	DECFSZ	INT_C,F
	GOTO	DIVI04
	BCF	T1CON,TMR1ON	;STOP
	BTFSC	PIR1,TMR1IF
	GOTO	NATURE11	;OVER FLOW
	
	MOVFW 	FF1
	MOVWF	AARGB0
	MOVFW 	FF2
	MOVWF	AARGB1
	MOVFW 	FF3
	MOVWF	AARGB2
	MOVFW 	FF4
	MOVWF	AARGB3
	MOVF 	TMR1H,W
	MOVWF	BARGB0
	MOVF 	TMR1L,W
  	MOVWF	BARGB1	
        
        CALL    FXD3216L
	MOVF	AARGB1,W
	SKPZ	
	GOTO	OVER_FLOW
	
DIVI06	
	MOVF	AARGB2,W
	MOVWF	AARGB0
	MOVWF	TEMPH	
	MOVF	AARGB3,W
	MOVWF	AARGB1
	MOVWF	TEMPL	
	CALL 	BIN2BCD16
	CALL	PRINT_RPM	


	MOVFW 	P_HIGH
	MOVWF	BARGB0
	MOVFW 	P_LOW
	MOVWF	BARGB1
	
	CALL  	FXM1616U
	BCF	STATUS,C
	RRF	AARGB0,F
	RRF	AARGB1,F
	RRF	AARGB2,F
	BCF	STATUS,C
	RRF	AARGB0,F
	RRF	AARGB1,F
	RRF	AARGB2,F

	MOVFW	AARGB1
	MOVWF	TEMPH	
	MOVFW	AARGB2
	MOVWF	TEMPL	
	CALL 	BIN2BCD16
	CALL	PRINT_KMH	
		
		
	
	MOVLW	D'30'	
	CALL	LOOP_L		
	GOTO	DIVI80
OVER_FLOW
	MOVLW	H'07'
	CALL	DDRAM_ADDRESS
	MOVLW   'O'
        CALL    PRINT_CHAR
	MOVLW   'v'
        CALL    PRINT_CHAR
	MOVLW   'e'
        CALL    PRINT_CHAR
	MOVLW   'r'
        CALL    PRINT_CHAR
	MOVLW   ' '
        CALL    PRINT_CHAR
	MOVLW	D'30'	
	CALL	LOOP_L	
	GOTO	DIVI80
;----------------------------------------
NATURE01
	CLRF	AARGB2
	CLRF	AARGB3
NATURE0
	MOVF	AARGB2,W
	MOVWF	AARGB0
	MOVWF	TEMPH	
	MOVF	AARGB3,W
	MOVWF	AARGB1
	MOVWF	TEMPL	
	CALL 	BIN2BCD16
	CALL	PRINT_RPM	

	MOVFW 	P_HIGH
	MOVWF	BARGB0
	MOVFW 	P_LOW
	MOVWF	BARGB1
	CALL  	FXM1616U
	BCF	STATUS,C
	RRF	AARGB0,F
	RRF	AARGB1,F
	RRF	AARGB2,F
	BCF	STATUS,C
	RRF	AARGB0,F
	RRF	AARGB1,F
	RRF	AARGB2,F
	MOVFW	AARGB1
	MOVWF	TEMPH	
	MOVFW	AARGB2
	MOVWF	TEMPL	
	CALL 	BIN2BCD16
	CALL	PRINT_KMH	
	MOVLW	D'30'	
	CALL	LOOP_L	
NATURE11
	BCF	T1CON,T1CKPS0
	BCF	T1CON,T1CKPS1	;PRESCALE 1
NAT01	
	CLRF	TMR1H
	CLRF	TMR1L
	BCF	PIR1,TMR1IF
NAT02
	BTFSC	PORTB,0
	GOTO 	NAT02
	BSF	T1CON,TMR1ON	;START
NAT03
	BTFSC	PIR1,TMR1IF
	GOTO	MAIN11		;OVER FLOW
	BTFSS	PORTA,0
	GOTO	P_SEL1	
	BTFSS	PORTA,1
	GOTO	T_SEL
	BTFSS	RA,5
	GOTO	START
	BTFSS	PORTB,0
	GOTO 	NAT03
	BCF	T1CON,TMR1ON	;STOP
	BCF	PIR1,TMR1IF
	CLRF	TMR1H
	CLRF	TMR1L
	BSF	T1CON,TMR1ON	;START
NAT04
	
	BTFSC	PORTB,0
	GOTO 	NAT04
		
NAT05
	BTFSC	PIR1,TMR1IF
	GOTO	MAIN11		;OVER FLOW
	BTFSS	PORTB,0
	GOTO 	NAT05
	BCF	T1CON,TMR1ON	;STOP
	BTFSC	PIR1,TMR1IF
	GOTO	MAIN11		;OVERFLOW
	
	MOVLW	D'12'
	SUBWF	TMR1H,W		
	SKPC			;F<W , C=0
	GOTO	DIVI80		;
		
	MOVFW 	EE1
	MOVWF	AARGB0
	MOVFW 	EE2
	MOVWF	AARGB1
	MOVFW 	EE3
	MOVWF	AARGB2
	MOVFW 	EE4
	MOVWF	AARGB3
	MOVF 	TMR1H,W
	MOVWF	BARGB0
	MOVF 	TMR1L,W
  	MOVWF	BARGB1	
       
        CALL    FXD3216L
		
	GOTO 	NATURE0
;--------------------------------------------


;------ END OF TACHOMETER --------------








;====================================================
DST_MAIN
	BCF	RA,4		;OUT +5V TURN ON
	BSF	FLAG_REG,F_TDST	;IF SET GOTO DST MODE
	CALL	CLEAR_DISPLAY	;CLEAR DISPLAY
	MOVLW	0
	CALL	DDRAM_ADDRESS
	MOVLW 	'S'		
	CALL    PRINT_CHAR	
	MOVLW 	'e'		
	CALL    PRINT_CHAR	
	MOVLW 	'r'		
	CALL    PRINT_CHAR	
	MOVLW 	'v'		
	CALL    PRINT_CHAR	
	MOVLW 	'o'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW	LDELAY		;DELAY............
	CALL 	LOOP_L	
	
;-------SETUP_INTERRUPT-------

	BSF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
        MOVLW 	B'10000111'
        MOVWF   OPTIONF         ;INTERNAT CLOCK,TRM0,1:256
	BCF     STATUS,RP0      ;ENABLE BANK0
	MOVLW   TMR1            ;256*78=19968 uS
	MOVWF	TMR0
	BCF	INTCON,TOIF
	BSF	INTCON,TOIE
	BSF	INTCON,GIE	;ENABLE INT

	MOVLW 	6
        MOVWF   MODE_SEL
;-----------------------------------------------
MODE_0
	BCF	INTCON,GIE	;DISABLE INT
	INCF 	MODE_SEL,F
	MOVLW	8		;MODE_SEL 1-7 ONLY
	SUBWF	MODE_SEL,W
	SKPZ
	GOTO	MODE_01
	MOVLW	1
	MOVWF	MODE_SEL

;-------ON MODE_SEL GOTO----------
MODE_01
	
	MOVLW	1		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	MODE_7		;KNOB
	MOVLW	2		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	MODE_3		;FSAT
	MOVLW	3		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	MODE_4		;MIDDLE
	MOVLW	4		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	MODE_2		;SLOW
	MOVLW	5		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	MODE_5		;RANDOM
	MOVLW	6		
	SUBWF	MODE_SEL,W
	SKPNZ
	GOTO	MODE_6		;ESCAPE
	GOTO	MODE_1		;MANUAL
;-------------------------------------------------	
MODE_7
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'K'		
	CALL    PRINT_CHAR
	MOVLW 	'n'		
	CALL    PRINT_CHAR	
	MOVLW 	'o'		
	CALL    PRINT_CHAR	
	MOVLW 	'b'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	BSF	INTCON,GIE	;ENABLE INT
MODE_70	
	BTFSS	FLAG_REG,F_INT	
	GOTO	MODE_70
	BCF	FLAG_REG,F_INT	
	BTFSS	RA,1
	GOTO	CENT_B7
	CALL	SET_AN3
	CALL	START_ADC
	MOVFW	ADRESH
	MOVWF	PWM_C
	GOTO	MODE_70
	
CENT_B7
	MOVLW	D'150'
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSC	RA,1	
	GOTO	MODE_70		
	BCF	ADCON0,ADON	;OFF ADC BEFORE CHANGE MODE
	GOTO	MODE_0
;---------------------------------------
MODE_1
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'M'		
	CALL    PRINT_CHAR
	MOVLW 	'a'		
	CALL    PRINT_CHAR	
	MOVLW 	'n'		
	CALL    PRINT_CHAR	
	MOVLW 	'u'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
		
MODE_10	
	BSF	INTCON,GIE	;ENABLE INT
	MOVLW	D'150'
	MOVWF	PWM_C
MODE_11
	BTFSS	RA,0
	GOTO 	INC_B
	BTFSS	RA,5
	GOTO	DEC_B
	BTFSS	RA,1
	GOTO	CENT_B
	CLRF	PUSH_C		
	GOTO	MODE_11
INC_B	MOVLW	RLIMIT
	SUBWF	PWM_C,W
	SKPNZ
	GOTO	MODE_11		;=RLIMIT, DO NOTHING
	INCF	PWM_C,F
	MOVLW	D'5'		;CONTINUE 5 STEP, ACCELERATION
	SUBWF	PUSH_C,W
	SKPNZ
	GOTO	INC_B1		;
	MOVLW	D'20'		;0.2 SECOND 
	CALL	LOOP_L		;CALL 0.1 SECOND LOOP
	INCF	PUSH_C,F
	GOTO	MODE_11
INC_B1				;FAST INC
	MOVLW	D'6'
	CALL	LOOP_L		;0.06 SECOND 
	GOTO	MODE_11	
DEC_B
	MOVLW	LLIMIT
	SUBWF	PWM_C,W
	SKPNZ
	GOTO	MODE_11		;=LLIMIT, DO NOTHING
	DECF	PWM_C,F
	MOVLW	D'5'		;CONTINUE 5 STEP, ACCELERATION
	SUBWF	PUSH_C,W
	SKPNZ
	GOTO	DEC_B1		;
	MOVLW	D'20'		;
	CALL	LOOP_L		;CALL 0.1 SECOND LOOP
	INCF	PUSH_C,F
	GOTO	MODE_11
DEC_B1				;FAST DEC
	MOVLW	D'6'
	CALL	LOOP_L		;0.06 SECOND
	GOTO	MODE_11	
CENT_B
	MOVLW	D'150'
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSS	RA,1	
	GOTO	MODE_0		;CHANGE MODE
	GOTO	MODE_10
;------------------------------------------------
MODE_2
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'S'		
	CALL    PRINT_CHAR
	MOVLW 	'l'		
	CALL    PRINT_CHAR	
	MOVLW 	'o'		
	CALL    PRINT_CHAR	
	MOVLW 	'w'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	

MODE_20				;SLOW 10 SECOND
	BSF	INTCON,GIE	;ENABLE INT
	MOVLW	D'150'
	MOVWF	PWM_C
	BSF	FLAG_REG,F_UP
MODE_21
	BTFSS	RA,0
	GOTO 	INC_B2
	BTFSS	RA,5
	GOTO	MODE_21
	BTFSS	RA,1
	GOTO	CENT_B2
	GOTO	MODE_21
INC_B2	
	MOVLW	2		; 2 TIMES
	MOVWF	PUSH_C
	MOVLW	5
	MOVWF	INT_C
INC_B21
	MOVF	INT_C,W		;5*100*20mS = 10 SECOND
	SKPZ
	GOTO	INC_B21

	MOVLW	5		;5*100*20mS = 10 SECOND
	MOVWF	INT_C	
	BTFSS	FLAG_REG,F_UP	;UP / DOWN ?
	GOTO	INC_B23		;COUNT_DOWN
INC_B22	
	MOVLW	RLIMIT		;UP
	SUBWF	PWM_C,W
	SKPNZ
	GOTO	INC_B24		;TURN_DOWN	
	INCF	PWM_C,F
	BTFSS	RA,5		;STOP ?
	GOTO	MODE_21
	GOTO	INC_B21
INC_B24 			;TURN_DOWN
	BCF	FLAG_REG,F_UP
INC_B23	
	MOVLW	LLIMIT		;
	SUBWF	PWM_C,W
	SKPNZ
	GOTO	INC_B25		;TURN_UP		
	DECF	PWM_C,F
	BTFSS	RA,5		;STOP ?
	GOTO	MODE_21
	GOTO	INC_B21
INC_B25
	BSF	FLAG_REG,F_UP
	DECF	PUSH_C,F
	SKPNZ
	GOTO 	MODE_20		;TESTING OVER
	GOTO	INC_B22
	
	

CENT_B2
	MOVLW	D'150'
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSS	RA,1	
	GOTO	MODE_0		;CHANGE MODE
	GOTO	MODE_20
	
;------------------------
MODE_3
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'F'		
	CALL    PRINT_CHAR
	MOVLW 	'a'		
	CALL    PRINT_CHAR	
	MOVLW 	's'		
	CALL    PRINT_CHAR	
	MOVLW 	't'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	
				;FAST 0.5 SECOND
MODE_30
	BSF	INTCON,GIE	;ENABLE INT
	MOVLW	D'150'
	MOVWF	PWM_C
MODE_31
	BTFSS	RA,0
	GOTO 	INC_B3
	BTFSS	RA,5
	GOTO	MODE_31
	BTFSS	RA,1
	GOTO	CENT_B3
	GOTO	MODE_31
INC_B3	
	MOVLW	5
	MOVWF	PUSH_C
INC_B31
	MOVLW	RLIMIT
	MOVWF	PWM_C
	BTFSS	RA,5
	GOTO	MODE_3
	MOVLW	D'45'		;0.45SECOD +22 INTERRUPT
	CALL	LOOP_L		;CALL 0.1 SECOND LOOP
	MOVLW	LLIMIT
	MOVWF	PWM_C
	BTFSS	RA,5
	GOTO	MODE_3
	MOVLW	D'45'
	CALL	LOOP_L		
	DECFSZ	PUSH_C,F
	GOTO	INC_B31
	GOTO	MODE_3

CENT_B3
	MOVLW	D'150'
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSS	RA,1	
	GOTO	MODE_0		;CHANGE MODE
	GOTO	MODE_30

;------------------------
MODE_4				;MID 2 SECOND
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'M'		
	CALL    PRINT_CHAR
	MOVLW 	'i'		
	CALL    PRINT_CHAR	
	MOVLW 	'd'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	
MODE_40
	BSF	INTCON,GIE	;ENABLE INT
	MOVLW	D'150'
	MOVWF	PWM_C
	BSF	FLAG_REG,F_UP
MODE_41
	BTFSS	RA,0
	GOTO 	INC_B4
	BTFSS	RA,5
	GOTO	MODE_41
	BTFSS	RA,1
	GOTO	CENT_B4
	GOTO	MODE_41
INC_B4	
	MOVLW	5		; 5 TIMES
	MOVWF	PUSH_C
	MOVLW	1
	MOVWF	INT_C
INC_B41
	MOVF	INT_C,W		;1*100*20mS = 2 SECOND
	SKPZ
	GOTO	INC_B41

	MOVLW	1		;1*100*20mS = 2 SECOND
	MOVWF	INT_C	
	BTFSS	FLAG_REG,F_UP	;UP / DOWN ?
	GOTO	INC_B43		;COUNT_DOWN
INC_B42	
	MOVLW	RLIMIT		;UP
	SUBWF	PWM_C,W
	SKPNZ
	GOTO	INC_B44		;TURN_DOWN	
	INCF	PWM_C,F
	BTFSS	RA,5		;STOP ?
	GOTO	MODE_41
	GOTO	INC_B41
INC_B44 			;TURN_DOWN
	BCF	FLAG_REG,F_UP
INC_B43	
	MOVLW	LLIMIT		;
	SUBWF	PWM_C,W
	SKPNZ
	GOTO	INC_B45		;TURN_UP		
	DECF	PWM_C,F
	BTFSS	RA,5		;STOP ?
	GOTO	MODE_41		
	GOTO	INC_B41
INC_B45
	BSF	FLAG_REG,F_UP
	DECF	PUSH_C,F
	SKPNZ
	GOTO 	MODE_40		;TESTING OVER
	GOTO	INC_B42

CENT_B4
	MOVLW	D'150'
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSS	RA,1	
	GOTO	MODE_0		;CHANGE MODE
	GOTO	MODE_40
;---------------------------------------------------------
	
MODE_5				;Random
	BSF	INTCON,GIE	;ENABLE INT
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'R'		
	CALL    PRINT_CHAR
	MOVLW 	'a'		
	CALL    PRINT_CHAR	
	MOVLW 	'n'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	
MODE_50
	BSF	INTCON,GIE	;ENABLE INT
	MOVLW	D'150'
	MOVWF	PWM_C
MODE_51
	DECFSZ 	RAND_LO,F	;RANDOM SEED GENERATOR
	GOTO	MODE_52
	DECF	RAND_LO,F		;MAKE SURE NO ZERO
	DECFSZ	RAND_HI,F
	GOTO	MODE_52
	DECF	RAND_HI,F		;MAKE SURE NO ZERO	
MODE_52	BTFSS	RA,0
	GOTO 	INC_B5
	BTFSS	RA,5
	GOTO	MODE_51
	BTFSS	RA,1
	GOTO	CENT_B5
	GOTO	MODE_51
INC_B5	
	MOVLW	H'20'		;20-30 RANDOM TEST	
	MOVWF	PUSH_C
INC_B51
	MOVLW	D'10'		;10 PULSES PER POSITION 
	MOVWF	INT_C	
	
;******************************
RANDOM16
	RLF     RAND_HI,W
	XORWF   RAND_HI,W
	RLF     TEMP1,F           
	SWAPF   RAND_HI,F
	SWAPF   RAND_LO,W
	RLF     TEMP1,F
	XORWF   RAND_HI,W        
	SWAPF   RAND_HI,F
	ANDLW   0x01
	RLF     RAND_LO,F
	XORWF   RAND_LO,F
	RLF     RAND_HI,F
;********************************

	
	MOVLW	RLIMIT
	SUBWF	RAND_LO,W
	SKPNC			;F<W , C=0
	GOTO	RANDOM16	
	MOVLW	LLIMIT
	SUBWF	RAND_LO,W
	SKPC			;F>W , C=1
	GOTO	RANDOM16			
	MOVF	RAND_LO,W
	MOVWF	PWM_C
INC_B52
	MOVF	INT_C,W		
	SKPZ
	GOTO	INC_B52
	BTFSS	RA,5		;STOP ?
	GOTO	MODE_5
	DECFSZ	PUSH_C,F
	GOTO	INC_B51
	GOTO	MODE_50		;TESTING OVER
CENT_B5
	MOVLW	D'150'
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSS	RA,1	
	GOTO	MODE_0		;CHANGE MODE
	GOTO	MODE_50
;---------------------------------
MODE_6
	BCF	INTCON,GIE	;ENABLE INT
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'<'		
	CALL    PRINT_CHAR
	MOVLW 	'E'		
	CALL    PRINT_CHAR	
	MOVLW 	'S'		
	CALL    PRINT_CHAR	
	MOVLW 	'C'		
	CALL    PRINT_CHAR	
	MOVLW 	'>'		
	CALL    PRINT_CHAR	

MODE_62	BTFSS	RA,0
	GOTO 	start
	BTFSS	RA,1
	GOTO	CENT_6
	GOTO	MODE_62

CENT_6
	MOVLW	D'150'
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSS	RA,1	
	GOTO	MODE_0		;CHANGE MODE
	GOTO	MODE_62



;------ END OF DST ---------------


;----------------------------------
;-----  SPEED CONTROLLER TESTER  --  
SCT_MAIN
	BCF	INTCON,GIE	;ENABLE INT
	BSF	RA,4		;OUT +5V OFF
	BSF	FLAG_REG,F_SCT	;IF SET GOTO DST MODE
	CALL	CLEAR_DISPLAY	;CLEAR DISPLAY
	MOVLW	0
	CALL	DDRAM_ADDRESS
	MOVLW 	'S'		
	CALL    PRINT_CHAR
	MOVLW 	'p'		
	CALL    PRINT_CHAR	
	MOVLW 	'e'		
	CALL    PRINT_CHAR	
	MOVLW 	'e'		
	CALL    PRINT_CHAR	
	MOVLW 	'd'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR
	MOVLW	LDELAY		;DELAY............
	CALL 	LOOP_L		
;-----------------------------------------------

	BSF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
        MOVLW 	B'10000111'
        MOVWF   OPTIONF         ;INTERNAT CLOCK,TRM0,1:256
	BCF     STATUS,RP0      ;ENABLE BANK0
	MOVLW   TMR1            ;256*78=19968 uS
	MOVWF	TMR0
	BCF	INTCON,TOIF
	BCF	INTCON,TOIE
	BCF	INTCON,GIE	;DISABLE INT

;
;-------------------------------------------------	

SCT_1
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'M'		
	CALL    PRINT_CHAR	
	MOVLW 	'a'		
	CALL    PRINT_CHAR	
	MOVLW 	'n'		
	CALL    PRINT_CHAR	
	MOVLW 	'u'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	BSF	INTCON,TOIE
	BSF	INTCON,GIE	;ENABLE INT	
	MOVLW	D'100'		;MINIMUM
	MOVWF	PWM_C
SCT_11
	BTFSS	RA,0
	GOTO 	ISCT_B
	BTFSS	RA,5
	GOTO	DSCT_B
	BTFSS	RA,1
	GOTO	DCENT_B
	CLRF	PUSH_C	
	GOTO	SCT_11
ISCT_B	MOVLW	RLIMIT
	SUBWF	PWM_C,W
	SKPNZ
	GOTO	SCT_11		;=RLIMIT, DO NOTHING
	INCF	PWM_C,F
	MOVLW	D'5'		;CONTINUE 5 STEP, ACCELERATION
	SUBWF	PUSH_C,W
	SKPNZ
	GOTO	ISCT_B1		;
	MOVLW	D'20'		;0.2 SECOND 
	CALL	LOOP_L		;CALL 0.1 SECOND LOOP
	INCF	PUSH_C,F
	GOTO	SCT_11
ISCT_B1				;FAST INC
	MOVLW	D'6'
	CALL	LOOP_L		;0.06 SECOND 
	GOTO	SCT_11	
DSCT_B
	MOVLW	LLIMIT
	SUBWF	PWM_C,W
	SKPNZ
	GOTO	SCT_11		;=LLIMIT, DO NOTHING
	DECF	PWM_C,F
	MOVLW	D'5'		;CONTINUE 5 STEP, ACCELERATION
	SUBWF	PUSH_C,W
	SKPNZ
	GOTO	DSCT_B1		;
	MOVLW	D'20'		;
	CALL	LOOP_L		;CALL 0.1 SECOND LOOP
	INCF	PUSH_C,F
	GOTO	SCT_11
DSCT_B1				;FAST DEC
	MOVLW	D'6'
	CALL	LOOP_L		;0.06 SECOND
	GOTO	SCT_11	
DCENT_B
	BCF	INTCON,TOIE
	BCF	INTCON,GIE	;DISABLE INT
	MOVLW	D'100'		;MINIMUM
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSS	RA,1	
	GOTO	SCT_0		;CHANGE MODE
	GOTO	SCT_1		;GOTO BEGINING, 1000uS
;--------------------------------------------------------
SCT_0
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'K'		
	CALL    PRINT_CHAR
	MOVLW 	'n'		
	CALL    PRINT_CHAR	
	MOVLW 	'o'		
	CALL    PRINT_CHAR	
	MOVLW 	'b'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	BSF	INTCON,TOIE
	BSF	INTCON,GIE	;ENABLE INT
SCT_01	
	BTFSS	FLAG_REG,F_INT		 
	GOTO 	SCT_01
	BCF	FLAG_REG,F_INT		
	
	BTFSS	RA,1
	GOTO	SCTCENT_B
	CALL	SET_AN3
	CALL	START_ADC
	MOVFW	ADRESH
	MOVWF	PWM_C
	GOTO	SCT_01
	
SCTCENT_B
	BCF	INTCON,TOIE
	BCF	INTCON,GIE	;DISABLE INT
	MOVLW	D'100'		;MINIMUM
	MOVWF	PWM_C
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSC	RA,1	
	GOTO	SCT_0		
;	BCF	ADCON0,ADON	;OFF ADC BEFORE CHANGE MODE
	GOTO	DESCMODE
;---------------------------------------


DESCMODE
	
	MOVLW 	H'6'		
	CALL	DDRAM_ADDRESS
	MOVLW 	'<'		
	CALL    PRINT_CHAR
	MOVLW 	'E'		
	CALL    PRINT_CHAR	
	MOVLW 	'S'		
	CALL    PRINT_CHAR	
	MOVLW 	'C'		
	CALL    PRINT_CHAR	
	MOVLW 	'>'		
	CALL    PRINT_CHAR	
DMODE_62	
	BTFSS	RA,0
	GOTO 	start
	BTFSS	RA,1
	GOTO	DCENT_6
	GOTO	DMODE_62


DCENT_6
	MOVLW	BDELAY		;0.01 SECOND * 40
	CALL	LOOP_L			
	BTFSS	RA,1	
	GOTO	SCT_1		;CHANGR MODE TO SCT MAUN
	GOTO	DMODE_62

;------ END OF SCT -----------------




;=======================================================
RECT_MAIN
	
	BSF	RA,4		;OUT +5V OFF
;	BSF	FLAG_REG,F_TDST	
	BSF	FLAG_REG,F_RECT	
	

RECT_MAIN2	
	CALL	CLEAR_DISPLAY	;CLEAR DISPLAY
	MOVLW	0
	CALL	DDRAM_ADDRESS
	MOVLW 	'R'		
	CALL    PRINT_CHAR
	MOVLW 	'e'		
	CALL    PRINT_CHAR	
	MOVLW 	'c'		
	CALL    PRINT_CHAR	
	MOVLW 	'_'		
	CALL    PRINT_CHAR	
	MOVLW 	'T'		
	CALL    PRINT_CHAR	


	BSF     STATUS,RP0      ;
	MOVLW 	B'10000111'
        MOVWF   OPTIONF         ;INTERNAT CLOCK,TRM0,1:256
;	MOVLW 	B'10000000'
;       MOVWF   OPTIONF         ;INTERNAT CLOCK,TRM0,1:2
	MOVLW 	B'00000011'	;SET RB,1 INPUT 
        MOVWF 	TRISB     	;
	BCF     STATUS,RP0      ;
REC_01	
	CLRF	PWM_C
	CLRF	TEMPH	
	CALL	SET_AN2
	CALL	START_ADC
	MOVLW	H'05'
	CALL	PRINT_VOLT
REC_0	
	MOVLW	D'4'
	MOVWF	INT_C
	CALL	PRINT_PWM
;	GOTO	REC_3
	BSF	INTCON,TOIE
	BSF	INTCON,GIE	;ENABLE INT TO DISPLAY XXX uS	
REC_1
;	BTFSS	FLAG_REG,F_INT		
;	GOTO 	REC_1
;	BCF	INTCON,GIE
;	BCF	INTCON,TOIE
;	BCF	FLAG_REG,F_INT	

	

REC_2
	CLRF	PWM_C
REC_3
	BTFSS	RA,5
	GOTO	start	
	MOVFW	INT_C
	SKPNZ
	GOTO	REC_01
	BTFSS	RB,1
	GOTO 	REC_3
REC_4
	NOP			;10 uS LOOP
	NOP
	NOP
	NOP
	INCF 	PWM_C,F
	SKPNZ
	GOTO 	REC_ERROR	;IF PWM_C > 255, ERROR
	BTFSC	RB,1
	GOTO	REC_4
	DECF	PWM_C,F		;CORRECT 1-20 uS
	GOTO 	REC_5
	
REC_ERROR			;IF > 255 DISPLAY 255
	MOVLW	H'FF'
	MOVWF	PWM_C
REC_5
	CALL	SET_AN2
	CALL	START_ADC
	MOVLW	H'05'
	CALL	PRINT_VOLT	

	GOTO 	REC_0

;------ END OF RECEIVER TX ---------------------

BONUS
	CALL	CLEAR_DISPLAY
	CALL	BONUS_WARN
	CALL	CLEAR_DISPLAY
	MOVLW	H'2'
	CALL	DDRAM_ADDRESS
		
	MOVLW 	'V'		
	CALL    PRINT_CHAR	
	MOVLW 	'o'		
	CALL    PRINT_CHAR	
	MOVLW 	'l'		
	CALL    PRINT_CHAR	
	MOVLW 	't'		
	CALL    PRINT_CHAR	
	MOVLW 	'm'		
	CALL    PRINT_CHAR	
	MOVLW 	'e'		
	CALL    PRINT_CHAR	
	MOVLW 	't'		
	CALL    PRINT_CHAR	
	MOVLW 	'e'		
	CALL    PRINT_CHAR	
	MOVLW 	'r'		
	CALL    PRINT_CHAR	
	CALL 	SET_AN2
	
BOUNS1	
	CALL	START_ADC
	MOVFW	ADRESH
	MOVWF	TEMPH	
	BSF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
	MOVFW	ADRESL
	BCF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
	MOVWF	TEMPL	
	CALL 	BIN2BCD16
	MOVLW	H'49'
	
	CALL	PRINT_VOLT	
	MOVLW	D'20'	
	CALL	LOOP_L		
	BTFSS	RA,5
	GOTO	start		
	GOTO	BOUNS1
;-------------------------------------------------
;-------------------------------------------------	







;----------------------------------------------
;=====  SUB ROUTINE REDIRECTIONS  ===================


PRINT_PWM2
	
	MOVF	PWM_C,W
	MOVWF 	TEMP5
	CALL 	BIN2BCD
;--------------------------------------------------
	MOVLW   D'11'
        CALL    DDRAM_ADDRESS 
	MOVLW   B'00110000'
        IORWF   DD1,1               
        IORWF   DD2,1
        IORWF   DD3,1
	
	MOVFW   DD1
        CALL    PRINT_CHAR
        MOVFW   DD2
        CALL    PRINT_CHAR
        MOVFW   DD3
        CALL    PRINT_CHAR
        MOVLW   '0'
        
        CALL    PRINT_CHAR
	MOVLW   'u'
        CALL    PRINT_CHAR
        
	GOTO	GRAPH1		;TEMP1~4
GRAPH2	
	GOTO 	PRINT_PWM3
;------------------------------------------

 ;***************************************************************************
 ; BIN2BCD16 - CONVERTS A 16-BIT BINARY NUMBER IN TEMPH:TEMPL INTO A 3 BYTE PACKED
 ; BCD NUMBER IN R0:R1:R2.
 ; R0 HOLDS THE 10 THOUSANDS DIGIT
 ; R1 HOLDS THE HUNDREDS AND THOUSANDS DIGITS
 ; R2 HOLDS THE ONES AND TENS DIGITS
 ;***************************************************************************
 ; INPUTS: TEMPH, TEMPL (16-BIT BINARY NUMBER)
 ; OUTPUTS: R0, R1, R2 (5 DIGIT BCD NUMBER)
 ; USED: COUNT
	
BIN2BCD162		; INITIALIZE VARIABLES
	BCF STATUS,C 	; CLEAR CARRY BIT
	MOVLW D'16'
	MOVWF COUNT 	; INIT BIT COUNTER
	CLRF R0 	; CLEAR OUTPUT
	CLRF R1 	; CLEAR OUTPUT
	CLRF R2 	; CLEAR OUTPUT
LOOP16A2
	RLF TEMPL,F 	; MULT BY 2, SHIFT MSB TO TEMPH
	RLF TEMPH,F 	; MULT BY 2, SHIFT MSB TO R2
	RLF R2,F 	; MULT BY 2, SHIFT MSB TO R1
	RLF R1,F 	; MULT BY 2, SHIFT MSB TO R0
	RLF R0,F 	; MULT BY 2, SHIFT MSB TO CARRY
	DECFSZ COUNT,F 	; DECREMENT BIT COUNTER
	GOTO ADJDEC2
	RETURN
ADJDEC2
	MOVLW R2
	MOVWF FSR 	; POINT TO R2
	CALL ADJBCD2
	
	MOVLW R1
	MOVWF FSR 	; POINT TO R1
	CALL ADJBCD2
	
	MOVLW R0
	MOVWF FSR 	; POINT TO R0
	CALL ADJBCD2
	
	GOTO LOOP16A2
	
ADJBCD2
	MOVLW 3
	ADDWF INDF,W 	; W = 3 + RN
	MOVWF TEMP 	; TEMP = 3+RN
	BTFSC TEMP,3 	;
	MOVWF INDF 	; RN=TEMP
	MOVLW 30 	; DECIMAL ADJUST?
	ADDWF INDF,W 	; W=30+RN
	MOVWF TEMP 	; TEMP=30+RN
	BTFSC TEMP,7
	MOVWF INDF 	; RN=TEMP
	
;	RETURN
	GOTO 	BIN2BCD163

;---------------------------------------	

;-------------------------------------	
;   BIN TO BCD CONVERT
;   W ->  DD1,DD2,DD3

BIN2BCD2
	MOVWF	DD3		;1	
	CLRF	DD2		;10
	CLRF	DD1		;100

H1	MOVLW	0X64		; 100
	SUBWF	DD3,F		; SUBTRACT 100 FROM NUMBER
	BTFSC	STATUS,C	; NEGATIVE RESULT?
	GOTO	H2		; NO? GOTO H2
	GOTO	H3		; YES? GOTO H3 (100'S DDD3)
H2	INCF	DD1,F		; INCREMENT DD1 REGISTER
	GOTO	H1		; & LOOP BACK FOR ANOTHER TEST
H3	MOVLW	0X64		; 100
	ADDWF	DD3,F		; ADD 100 BACK TO NUMBER

T1	MOVLW	0X0A		; 10
	SUBWF	DD3,F		; SUBTRACT 10 FROM NUMBER
	BTFSC	STATUS,C	; NEGATIVE RESULT?
	GOTO	T2		; NO? GOTO T2
	GOTO	T3		; YES? GOTO T3 (10'S DDD3)
T2	INCF	DD2,F		; INCREMENT DD2 REGISTER
	GOTO	T1		; & LOOP BACK FOR ANOTHER TEST
T3	MOVLW	0X0A		; 10
	ADDWF	DD3,F		; ADD 10 BACK TO NUMBER
	GOTO 	BIN2BCD3

;-------------------------------------------------


PRINT_RPM2
	MOVLW	H'07'
	CALL	DDRAM_ADDRESS
	MOVF	R0,W
	SKPZ
	GOTO 	RPM5
	MOVLW   ' '
        CALL    PRINT_CHAR
	GOTO 	RPM4
RPM5	
	MOVF	R0,W
	MOVLW   B'00110000'
        IORWF   R0,W   
	CALL    PRINT_CHAR
	SWAPF   R1,W		;4
        ANDLW   B'00001111'
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	MOVF	R1,W		;3
	ANDLW   B'00001111'     ;      
	IORLW   B'00110000'
	CALL    PRINT_CHAR
	SWAPF   R2,W		;2
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	GOTO 	RPM1
RPM4	
	SWAPF   R1,W		;SWAP HALVES F
        ANDLW   B'00001111'
	SKPZ	
	GOTO 	RPM41
	MOVLW   ' '
        CALL    PRINT_CHAR
	GOTO	RPM3     ;      
RPM41	
	SWAPF   R1,W		;SWAP HALVES F
        ANDLW   B'00001111'
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	MOVF	R1,W		;3
	ANDLW   B'00001111'     ;      
	IORLW   B'00110000'
	CALL    PRINT_CHAR
	SWAPF   R2,W		;2
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	GOTO 	RPM1
RPM3	
	MOVF	R1,W
	ANDLW   B'00001111'     ;      
	SKPZ
	GOTO	RPM31
	MOVLW   ' '
        CALL    PRINT_CHAR
	GOTO	RPM2
RPM31
	MOVF	R1,W
	ANDLW   B'00001111' 
	IORLW   B'00110000'
	CALL    PRINT_CHAR
	SWAPF   R2,W		;2
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	GOTO 	RPM1
RPM2	
	SWAPF   R2,W		;SWAP HALVES F
        ANDLW   B'00001111'     ; 
	SKPZ
	GOTO 	RPM21
	MOVLW   ' '
        CALL    PRINT_CHAR
	GOTO 	RPM1
RPM21     
	SWAPF   R2,W		;SWAP HALVES F
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
RPM1
	MOVF	R2,W
	ANDLW   B'00001111'     ;      
	IORLW   B'00110000'
	CALL    PRINT_CHAR 
	MOVLW   ' '
        CALL    PRINT_CHAR
	MOVLW   'R'
        CALL    PRINT_CHAR
	MOVLW   'P'
        CALL    PRINT_CHAR
	MOVLW   'M'
        CALL    PRINT_CHAR
	GOTO 	PRINT_RPM3

;--------------------------------------
;------------------------------------------------------------


PRINT_KMH2
	MOVLW	H'46'
	CALL	DDRAM_ADDRESS
	MOVF	R0,W
	SKPZ
	GOTO 	KMH5
	MOVLW   ' '
        CALL    PRINT_CHAR
	GOTO 	KMH4
KMH5	
	MOVF	R0,W
	MOVLW   B'00110000'
        IORWF   R0,W   
	CALL    PRINT_CHAR
	SWAPF   R1,W		;4
        ANDLW   B'00001111'
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	MOVF	R1,W		;3
	ANDLW   B'00001111'     ;      
	IORLW   B'00110000'
	CALL    PRINT_CHAR
	MOVLW   '.'
        CALL    PRINT_CHAR
	SWAPF   R2,W		;2
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	GOTO 	KMH1
KMH4	
	SWAPF   R1,W		;SWAP HALVES F
        ANDLW   B'00001111'
	SKPZ	
	GOTO 	KMH41
	MOVLW   ' '
        CALL    PRINT_CHAR
	GOTO	KMH3     ;      
KMH41	
	SWAPF   R1,W		;SWAP HALVES F
        ANDLW   B'00001111'
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	MOVF	R1,W		;3
	ANDLW   B'00001111'     ;      
	IORLW   B'00110000'
	CALL    PRINT_CHAR
	MOVLW   '.'
        CALL    PRINT_CHAR
	SWAPF   R2,W		;2
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	GOTO 	KMH1
KMH3	
	MOVF	R1,W
	ANDLW   B'00001111'     ;      
	SKPZ
	GOTO	KMH31
	MOVLW   '0'
        CALL    PRINT_CHAR
        MOVLW   '.'
        CALL    PRINT_CHAR
        SWAPF   R2,W		;2
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	GOTO	KMH1
KMH31
	MOVF	R1,W
	ANDLW   B'00001111' 
	IORLW   B'00110000'
	CALL    PRINT_CHAR
	MOVLW   '.'
        CALL    PRINT_CHAR
	SWAPF   R2,W		;2
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	GOTO 	KMH1
KMH1
	MOVLW   ' '
        CALL    PRINT_CHAR
	MOVLW   'K'
        CALL    PRINT_CHAR
	MOVLW   'm'
        CALL    PRINT_CHAR
	MOVLW   '/'
        CALL    PRINT_CHAR
	MOVLW   'H'
        CALL    PRINT_CHAR
	GOTO 	PRINT_KMH3

;--------------------------------------------------
MAKEWORD
	MOVLW 	H'08'		
	CALL	CGRAM_ADDRESS
	MOVLW 	B'00010000'		
	CALL    PRINT_CHAR
	MOVLW 	B'00010000'		
	CALL    PRINT_CHAR	
	MOVLW 	B'00010000'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00011111'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00010000'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00010000'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00010000'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00010000'	
	CALL    PRINT_CHAR	;END OF WORD 1, LEFT
	MOVLW 	B'00000001'		
	CALL    PRINT_CHAR
	MOVLW 	B'00000001'		
	CALL    PRINT_CHAR	
	MOVLW 	B'00000001'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00011111'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00000001'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00000001'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00000001'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00000001'	
	CALL    PRINT_CHAR	;END OF WORD 2, RIGHT
	MOVLW 	B'00000100'		
	CALL    PRINT_CHAR
	MOVLW 	B'00000100'		
	CALL    PRINT_CHAR	
	MOVLW 	B'00000100'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00011111'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00000100'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00000100'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00000100'	
	CALL    PRINT_CHAR	
	MOVLW 	B'00000100'	
	CALL    PRINT_CHAR	;END OF WORD 3, CENTRAL
	GOTO	MAIN_M
;------------------------------------------------------
GRAPH1				;PLOT A SCALE BAR
	MOVF	PWM_C,W
	MOVWF	TEMP1
	MOVLW	D'102'
	SUBWF	TEMP1,W
	SKPNC			;F<W, C=0
	GOTO 	GRAPH_21	;F>=W
	MOVLW 	H'40'		;PWM < 102
	CALL	DDRAM_ADDRESS
	MOVLW 	'<'		
	CALL    PRINT_CHAR
	MOVLW	D'15'
	MOVWF	TEMP2
GRAPH_11			
	MOVLW 	'-'		
	CALL    PRINT_CHAR
	DECFSZ	TEMP2,F
	GOTO	GRAPH_11
	GOTO 	GRAPH_99
;-----------------------------------
GRAPH_21			;;PWM >= 102
	MOVLW	D'150'
	SUBWF	TEMP1,W
	SKPZ			;
	GOTO 	GRAPH_31
	MOVLW 	H'40'		;PWM=150
	CALL	DDRAM_ADDRESS
	
	MOVLW	D'7'
	MOVWF	TEMP2
GRAPH_22			
	MOVLW 	'-'		
	CALL    PRINT_CHAR
	DECFSZ	TEMP2,F
	GOTO	GRAPH_22
	MOVLW 	2		
	CALL    PRINT_CHAR
	MOVLW 	1		
	CALL    PRINT_CHAR
	MOVLW	D'7'
	MOVWF	TEMP2
GRAPH_23		
	MOVLW 	'-'		
	CALL    PRINT_CHAR
	DECFSZ	TEMP2,F
	GOTO	GRAPH_23
	GOTO	GRAPH_99
;----------------------------------
GRAPH_31	
	MOVLW	D'199'
	SUBWF	TEMP1,W
	SKPC			;F>=W, C=1
	GOTO 	GRAPH_41
	MOVLW 	H'40'		;PWM >= 199
	CALL	DDRAM_ADDRESS
	MOVLW	D'15'
	MOVWF	TEMP2
GRAPH_32		
	MOVLW 	'-'		
	CALL    PRINT_CHAR
	DECFSZ	TEMP2,F
	GOTO	GRAPH_32
	MOVLW 	'>'		
	CALL    PRINT_CHAR
	GOTO	GRAPH_99
;----------------------------------------

GRAPH_41			;101<PWM<199 , <>150
	CLRF	TEMP3
	MOVLW	D'152'
	SUBWF	TEMP1,W
	SKPNC			;F<W, C=0
	DECF	TEMP1,F		;IF >= 152, TEMP1 -1	

	MOVLW	D'102'		;PWM -102
	SUBWF	TEMP1,F
GRAPH_42	
	MOVLW	D'6'
	SUBWF	TEMP1,W
	SKPC			;F<W, C=0
	GOTO 	GRAPH_51
	INCF	TEMP3,F	;"-"S BEFORE INDECATOR
	MOVLW	D'6'
	SUBWF	TEMP1,F
	GOTO	GRAPH_42
GRAPH_51	
	MOVLW	D'15'
	MOVWF	TEMP4	
	MOVF	TEMP3,W
	SUBWF	TEMP4,F	;"-"S AFTER INDECATOR
	MOVLW 	H'40'		
	CALL	DDRAM_ADDRESS
GRAPH_52	
	MOVF	TEMP3,W
	SKPNZ
	GOTO	GRAPH_53
	MOVLW 	'-'		
	CALL    PRINT_CHAR
	DECF	TEMP3,F
	GOTO	GRAPH_52
;--------------------------------
GRAPH_53
	MOVLW	D'2'
	SUBWF	TEMP1,W
	SKPNC			;F<W, C=0
	GOTO	GRAPH_54
	MOVLW 	1		
	CALL    PRINT_CHAR
	GOTO 	GRAPH_61
GRAPH_54	
	MOVLW	D'4'
	SUBWF	TEMP1,W
	SKPNC			;F<W, C=0
	GOTO	GRAPH_55
	MOVLW 	3		
	CALL    PRINT_CHAR
	GOTO 	GRAPH_61
GRAPH_55	
	MOVLW 	2		
	CALL    PRINT_CHAR
;--------------------------------
GRAPH_61
	MOVF	TEMP4,W
	SKPNZ
	GOTO	GRAPH_99
	MOVLW 	'-'		
	CALL    PRINT_CHAR
	DECF	TEMP4,F
	GOTO	GRAPH_61

GRAPH_99
	GOTO 	GRAPH2
;--------------------------------------------------------

PRINT_VOLT2
	CALL	DDRAM_ADDRESS	;W TO DDRAM ADDRESS
	MOVFW	ADRESH
	MOVWF	TEMPH	
	BSF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
	MOVFW	ADRESL
	BCF     STATUS,RP0      ;SET 3,5 ENABLE BANK1
	MOVWF	TEMPL	
	CALL 	BIN2BCD16

	MOVF	R1,W
	ANDLW   B'00001111'     ; 3     
	SKPZ
	GOTO	VOLT31
	MOVLW   ' '
        CALL    PRINT_CHAR
	GOTO	VOLT2
VOLT31
	MOVF	R1,W
	ANDLW   B'00001111' 	;3
	IORLW   B'00110000'
	CALL    PRINT_CHAR
	SWAPF   R2,W		;2
        ANDLW   B'00001111'     ; 
	IORLW   B'00110000'
        CALL    PRINT_CHAR
	GOTO 	VOLT1
VOLT2	
	SWAPF   R2,W		;SWAP HALVES F
        ANDLW   B'00001111'     ;2 
        IORLW   B'00110000'
        CALL    PRINT_CHAR
VOLT1
	MOVLW   '.'
        CALL    PRINT_CHAR
	MOVF	R2,W
	ANDLW   B'00001111'     ;      
	IORLW   B'00110000'
	CALL    PRINT_CHAR 
;	MOVLW   ' '
;       CALL    PRINT_CHAR
	MOVLW   'V'
        CALL    PRINT_CHAR
	
	GOTO 	PRINT_VOLT3

;--------------------------------------		
BONUS_WARN2	
	
	MOVLW	0
	CALL	DDRAM_ADDRESS
	MOVLW 	'Y'		
	CALL    PRINT_CHAR
	MOVLW 	'o'		
	CALL    PRINT_CHAR	
	MOVLW 	'u'		
	CALL    PRINT_CHAR
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	'N'		
	CALL    PRINT_CHAR	
	MOVLW 	'O'		
	CALL    PRINT_CHAR	
	MOVLW 	'T'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR
	MOVLW 	's'		
	CALL    PRINT_CHAR	
	MOVLW 	'u'		
	CALL    PRINT_CHAR	
	MOVLW 	'p'		
	CALL    PRINT_CHAR	
	MOVLW 	'p'		
	CALL    PRINT_CHAR	
	MOVLW 	'o'		
	CALL    PRINT_CHAR	
	MOVLW 	's'		
	CALL    PRINT_CHAR	
	MOVLW 	'e'		
	CALL    PRINT_CHAR
	MOVLW 	'd'	
	CALL    PRINT_CHAR
		
	MOVLW	H'44'
	CALL	DDRAM_ADDRESS
	
	MOVLW 	't'		
	CALL    PRINT_CHAR	
	MOVLW 	'o'		
	CALL    PRINT_CHAR
	MOVLW 	' '		
	CALL    PRINT_CHAR
	MOVLW 	'b'		
	CALL    PRINT_CHAR	
	MOVLW 	'e'		
	CALL    PRINT_CHAR	
	MOVLW 	' '		
	CALL    PRINT_CHAR	
	MOVLW 	'H'		
	CALL    PRINT_CHAR	
	MOVLW 	'E'		
	CALL    PRINT_CHAR	
	MOVLW 	'R'		
	CALL    PRINT_CHAR	
	MOVLW 	'E'		
	CALL    PRINT_CHAR
	MOVLW   D'250'
        CALL    LOOP_L	
        GOTO 	BONUS_WARN3
;---------------------------------
	

	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
;=============================================================	
	ORG	0X900	
	

;--------------------------------------------

;       32/16 Bit Unsigned Fixed Point Divide 32/16 -> 32.16

;       Input:  32 bit unsigned fixed point dividend in AARGB0, AARGB1,AARGB2,AARGB3
;               16 bit unsigned fixed point divisor in BARGB0, BARGB1

;       Use:    CALL    FXD3216U

;       Output: 32 bit unsigned fixed point quotient in AARGB0, AARGB1,AARGB2,AARGB3
;               16 bit unsigned fixed point remainder in REMB0, REMB1

;       Result: AARG, REM  <--  AARG / BARG

;       Max Timing:     2+699+2 = 703 clks

;       Max Timing:     2+663+2 = 667 clks

;       PM: 2+240+1 = 243               DM: 9


;UDIV3216L       ;macro

;       Max Timing:     16+6*22+21+21+6*22+21+21+6*22+21+21+6*22+21+8 = 699 clks

;       Min Timing:     16+6*21+20+20+6*21+20+20+6*21+20+20+6*21+20+3 = 663 clks

;       PM: 240                                 DM: 9
FXD3216L2
                CLRF            TEMP
		CLRF    	REMB0
        	CLRF    	REMB1
                RLF             AARGB0,W
                RLF             REMB1, F
                MOVF            BARGB1,W
                SUBWF           REMB1, F
                MOVF            BARGB0,W
                BTFSS           _C
                INCFSZ          BARGB0,W
                SUBWF           REMB0, F
                CLRW
                BTFSS           _C
                MOVLW           1
                SUBWF           TEMP, F
                RLF             AARGB0, F

                MOVLW           7
                MOVWF           LOOPCOUNT

LOOPU3216A      RLF             AARGB0,W
                RLF             REMB1, F
                RLF             REMB0, F
                RLF             TEMP, F
                MOVF            BARGB1,W
                BTFSS           AARGB0,LSB
                GOTO            UADD26LA

                SUBWF           REMB1, F
                MOVF            BARGB0,W
                BTFSS           _C
                INCFSZ          BARGB0,W
                SUBWF           REMB0, F
                CLRW
                BTFSS           _C
                MOVLW           1
                SUBWF           TEMP, F
                GOTO            UOK26LA

UADD26LA        ADDWF           REMB1, F
                MOVF            BARGB0,W
                BTFSC           _C
                INCFSZ          BARGB0,W
                ADDWF           REMB0, F
                CLRW
                BTFSC           _C
                MOVLW           1
                ADDWF           TEMP, F
        
UOK26LA RLF             AARGB0, F

                DECFSZ          LOOPCOUNT, F
                GOTO            LOOPU3216A

                RLF             AARGB1,W
                RLF             REMB1, F
                RLF             REMB0, F
                RLF             TEMP, F
                MOVF            BARGB1,W
                BTFSS           AARGB0,LSB
                GOTO            UADD26L8

                SUBWF           REMB1, F
                MOVF            BARGB0,W
                BTFSS           _C
                INCFSZ          BARGB0,W
                SUBWF           REMB0, F
                CLRW
                BTFSS           _C
                MOVLW           1
                SUBWF           TEMP, F
                GOTO            UOK26L8

UADD26L8        ADDWF           REMB1, F
                MOVF            BARGB0,W
                BTFSC           _C
                INCFSZ          BARGB0,W
                ADDWF           REMB0, F
                CLRW
                BTFSC           _C
                MOVLW           1
                ADDWF           TEMP, F
        
UOK26L8 RLF             AARGB1, F

                MOVLW           7
                MOVWF           LOOPCOUNT

LOOPU3216B      RLF             AARGB1,W
                RLF             REMB1, F
                RLF             REMB0, F
                RLF             TEMP, F
                MOVF            BARGB1,W
                BTFSS           AARGB1,LSB
                GOTO            UADD26LB

                SUBWF           REMB1, F
                MOVF            BARGB0,W
                BTFSS           _C
                INCFSZ          BARGB0,W
                SUBWF           REMB0, F
                CLRW
                BTFSS           _C
                MOVLW           1
                SUBWF           TEMP, F
                GOTO            UOK26LB

UADD26LB        ADDWF           REMB1, F
                MOVF            BARGB0,W
                BTFSC           _C
                INCFSZ          BARGB0,W
                ADDWF           REMB0, F
                CLRW
                BTFSC           _C
                MOVLW           1
                ADDWF           TEMP, F
        
UOK26LB RLF             AARGB1, F

                DECFSZ          LOOPCOUNT, F
                GOTO            LOOPU3216B

                RLF             AARGB2,W
                RLF             REMB1, F
                RLF             REMB0, F
                RLF             TEMP, F
                MOVF            BARGB1,W
                BTFSS           AARGB1,LSB
                GOTO            UADD26L16

                SUBWF           REMB1, F
                MOVF            BARGB0,W
                BTFSS           _C
                INCFSZ          BARGB0,W
                SUBWF           REMB0, F
                CLRW
                BTFSS           _C
                MOVLW           1
                SUBWF           TEMP, F
                GOTO            UOK26L16

UADD26L16       ADDWF           REMB1, F
                MOVF            BARGB0,W
                BTFSC           _C
                INCFSZ          BARGB0,W
                ADDWF           REMB0, F
                CLRW
                BTFSC           _C
                MOVLW           1
                ADDWF           TEMP, F
        
UOK26L16        RLF             AARGB2, F

                MOVLW           7
                MOVWF           LOOPCOUNT

LOOPU3216C      RLF             AARGB2,W
                RLF             REMB1, F
                RLF             REMB0, F
                RLF             TEMP, F
                MOVF            BARGB1,W
                BTFSS           AARGB2,LSB
                GOTO            UADD26LC

                SUBWF           REMB1, F
                MOVF            BARGB0,W
                BTFSS           _C
                INCFSZ          BARGB0,W
                SUBWF           REMB0, F
                CLRW
                BTFSS           _C
                MOVLW           1
                SUBWF           TEMP, F
                GOTO            UOK26LC

UADD26LC        ADDWF           REMB1, F
                MOVF            BARGB0,W
                BTFSC           _C
                INCFSZ          BARGB0,W
                ADDWF           REMB0, F
                CLRW
                BTFSC           _C
                MOVLW           1
                ADDWF           TEMP, F
        
UOK26LC RLF             AARGB2, F

                DECFSZ          LOOPCOUNT, F
                GOTO            LOOPU3216C

                RLF             AARGB3,W
                RLF             REMB1, F
                RLF             REMB0, F
                RLF             TEMP, F
                MOVF            BARGB1,W
                BTFSS           AARGB2,LSB
                GOTO            UADD26L24

                SUBWF           REMB1, F
                MOVF            BARGB0,W
                BTFSS           _C
                INCFSZ          BARGB0,W
                SUBWF           REMB0, F
                CLRW
                BTFSS           _C
                MOVLW           1
                SUBWF           TEMP, F
                GOTO            UOK26L24

UADD26L24       ADDWF           REMB1, F
                MOVF            BARGB0,W
                BTFSC           _C
                INCFSZ          BARGB0,W
                ADDWF           REMB0, F
                CLRW
                BTFSC           _C
                MOVLW           1
                ADDWF           TEMP, F
        
UOK26L24        RLF             AARGB3, F

                MOVLW           7
                MOVWF           LOOPCOUNT

LOOPU3216D      RLF             AARGB3,W
                RLF             REMB1, F
                RLF             REMB0, F
                RLF             TEMP, F
                MOVF            BARGB1,W
                BTFSS           AARGB3,LSB
                GOTO            UADD26LD

                SUBWF           REMB1, F
                MOVF            BARGB0,W
                BTFSS           _C
                INCFSZ          BARGB0,W
                SUBWF           REMB0, F
                CLRW
                BTFSS           _C
                MOVLW           1
                SUBWF           TEMP, F
                GOTO            UOK26LD

UADD26LD        ADDWF           REMB1, F
                MOVF            BARGB0,W
                BTFSC           _C
                INCFSZ          BARGB0,W
                ADDWF           REMB0, F
                CLRW
                BTFSC           _C
                MOVLW           1
                ADDWF           TEMP, F
        
UOK26LD RLF             AARGB3, F

                DECFSZ          LOOPCOUNT, F
                GOTO            LOOPU3216D

                BTFSC           AARGB3,LSB
                GOTO            UOK26L
                MOVF            BARGB1,W
	        ADDWF           REMB1, F
                MOVF            BARGB0,W
                BTFSC           _C
                INCFSZ          BARGB0,W
                ADDWF           REMB0, F
UOK26L

                BCF	PCLATH,4	;GOTO MEMORY PAGE 0
		BCF	PCLATH,3
		GOTO 	FXD3216L3


;--------------------------------------
;************************************************************************
        
;       16x16 Bit Unsigned Fixed Point Multiply 16x16 -> 32

;       Input:  16 bit unsigned fixed point multiplicand in AARGB0
;                       16 bit unsigned fixed point multiplier in BARGB0

;       Use:    CALL    FXM1616U

;       Output: 32 bit unsigned fixed point product in AARGB0

;       Result: AARGB 0~3  <--  AARGB 0~1 x BARGB 0~1

;       Max Timing:     6+248+2 = 256 clks

;       Min Timing:     6+101 = 107 clks

;       PM: 6+51+1 = 58              DM: 9

FXM1616U2

                CLRF    AARGB2          ; clear partial product
                CLRF    AARGB3
                MOVF   AARGB0,W
                MOVWF   TEMPB0
                MOVF   AARGB1,W
                MOVWF   TEMPB1

;UMUL1616L        macro

;       Max Timing:     2+13+6*15+14+2+7*16+15 = 248 clks

;       Min Timing:     2+7*6+5+1+7*6+5+4 = 101 clks

;       PM: 51            DM: 9

                MOVLW   0x08
                MOVWF   LOOPCOUNT

LOOPUM1616A
                RRF     BARGB1, F
                BTFSC   _C
                GOTO    ALUM1616NAP
                DECFSZ  LOOPCOUNT, F
                GOTO    LOOPUM1616A

                MOVWF   LOOPCOUNT

LOOPUM1616B
                RRF     BARGB0, F
                BTFSC   _C
                GOTO    BLUM1616NAP
                DECFSZ  LOOPCOUNT, F
                GOTO    LOOPUM1616B

                CLRF    AARGB0
                CLRF    AARGB1
                
		BCF	PCLATH,4	;GOTO MEMORY PAGE 0
		BCF	PCLATH,3
		GOTO 	FXM1616U3

BLUM1616NAP
                BCF     _C
                GOTO    BLUM1616NA

ALUM1616NAP
                BCF     _C
                GOTO    ALUM1616NA

ALOOPUM1616
                RRF     BARGB1, F
                BTFSS   _C
                GOTO    ALUM1616NA
                MOVF   TEMPB1,W
                ADDWF   AARGB1, F
                MOVF            TEMPB0,W
                BTFSC           _C
                INCFSZ          TEMPB0,W
                ADDWF           AARGB0, F

ALUM1616NA
                RRF    AARGB0, F
                RRF    AARGB1, F
                RRF    AARGB2, F
                DECFSZ  LOOPCOUNT, F
                GOTO    ALOOPUM1616

                MOVLW   0x08
                MOVWF   LOOPCOUNT

BLOOPUM1616
                RRF             BARGB0, F
                BTFSS   _C
                GOTO    BLUM1616NA
                MOVF   TEMPB1,W
                ADDWF   AARGB1, F
                MOVF            TEMPB0,W
                BTFSC           _C
                INCFSZ          TEMPB0,W
                ADDWF           AARGB0, F

BLUM1616NA
                RRF    AARGB0, F
                RRF    AARGB1, F
                RRF    AARGB2, F
                RRF             AARGB3, F
                DECFSZ  LOOPCOUNT, F
                GOTO    BLOOPUM1616


		BCF	PCLATH,4	;GOTO MEMORY PAGE 0
		BCF	PCLATH,3
		GOTO 	FXM1616U3
;-------------------------------------------------------------


        END