;****************M.M.Electronics 2005****************
;*************Laboratorio di automatica**************
;**********Università degli studi di Roma************
;*******************"La Sapienza"********************
;************Rev. 1.a --MME-- 03-07-2005*************
;****************http://www.mmetft.it****************
WREG	EQU 0x00
TMR0	EQU 0x01
PCL		EQU 0x02
STATUS	EQU 0x03
FSR		EQU 0x04
Port_A	EQU 0x05
Port_B	EQU 0x06
Port_C	EQU 0x07
TMR1L	EQU 0x0E
TMR1H	EQU 0x0F
T1CON	EQU 0x10
T2CON	EQU 0x12
SSPBUF	EQU 0x13
SSPCON1	EQU 0x14
CCPR1L	EQU 0x15
CCPR2H	EQU 0x16
EEDATA	EQU 0x10C
EEADR	EQU 0x10D
EEDATH	EQU 0x10E
EEADRH	EQU 0x10F
PCLATH	EQU 0x0A
Intcon	EQU 0x0B
PIR1	EQU 0X0C
CCP1CON	EQU 0x17
OPT		EQU 0x81
TRISA	EQU 0x85
TRISB	EQU 0x86
TRISC	EQU 0x87
PIE1	EQU 0X8C
SSPCON2	EQU 0x91
PR2		EQU 0x92
SSPADD	EQU 0x93
SSPSTAT EQU 0x94	;Registro controllo BUS I2C
ADRESH	EQU 0x1E
ADCON0	EQU 0X1F
ADCON1	EQU 0x9F
EECON1	EQU 0x18C
EECON2	EQU 0x18D
Cnt		EQU 0x20
Ctrl	EQU 0x21	;Variabile controllo funzioni (**)
e_ctrl	EQU 0x22
VsVar	EQU 0x23
Tmp		EQU 0x24
Aux		EQU 0x25
Write	EQU 0x26
D0		EQU 0x27
D1		EQU 0x28
D2		EQU 0x29
Pos		EQU 0x2A
Ind		EQU 0x2B
MME		EQU 0x2C	;Variabile controllo menu(s)
enc		EQU 0x2D
Mn_Cnt	EQU 0x2E
enca0	EQU 0x2F
enca1	EQU 0x30
enca2	EQU 0x31
enca3	EQU 0x32
encb0	EQU 0x33
encb1	EQU 0x34
encb2	EQU 0x35
encb3	EQU 0x36
err_n0	EQU 0x37
err_n1	EQU 0x38
err_n2	EQU 0x39
err_n3	EQU 0x3A
tev_l	EQU 0x3B
tev_h	EQU 0x3C
TmpEU	EQU 0x3D
TmpED	EQU 0x3E
err_p0	EQU 0x3F
err_p1	EQU 0x40
err_p2	EQU 0X41
err_p3	EQU	0x42
ki_r0	EQU 0x43
ki_r1	EQU 0X44
ki_r2	EQU 0X45
ki_r3	EQU 0x46
ki_r4	EQU 0x47
ripp	EQU 0x48
PCL_Tmp	EQU 0x49
STA_Tmp	EQU	0x4A
W_Tmp	EQU 0x4B
CntEnc	EQU 0x4C
K		EQU 0x4D
opb0	EQU 0x4E
opb1	EQU 0x4F
opb2	EQU 0x50
opb3	EQU 0x51
opa0	EQU 0x52
opa1	EQU 0x53
opa2	EQU 0x54
opa3	EQU 0x55
opa4	EQU 0x56
opa5	EQU 0x57
opa6	EQU 0x58
tmp0	EQU 0x59
tmp1	EQU 0x5A
tmp2	EQU 0x5B
tmp3	EQU 0x5C
tmp4	EQU 0x5D
tmp5	EQU 0x5E
tmp6	EQU 0x5F
kp_r0	EQU 0x60
kp_r1	EQU 0x61
kp_r2	EQU 0x62
kp_r3	EQU 0x63
kp_r4	EQU 0x64
;
PIDSk	EQU 0x65
;*******Bank 1 register*******
temp	EQU 0xA1
count	EQU 0xA2
RI0		EQU 0xA3
RI1		EQU 0xA4
;R2		EQU 0xA5
;R3		EQU 0xA6
;R4		EQU 0xA7
byte0	EQU 0xA8
byte1	EQU 0xA9
;byte2	EQU 0xAA
;byte3	EQU 0xAB
RF0		EQU 0xAC
RF1		EQU 0xAD
RF2		EQU 0xAE
;*******Bank 1 register*******
		Org 0000h
Ini		Bsf PCLATH,4	;Seleziona pagina 2
		Bcf PCLATH,3	;memoria programma (1000h -17FFh)
		Goto Init
Int		Org 0004h		;Gestione interrupt
		;Bsf PCLATH,4	;Seleziona pagina 2
;*******Salvataggio registri*******
		;Bcf EPAB,0
		;Bcf Ctrl,3
		;Btfsc Port_B,0
		;Bsf EPAB,0
		;Btfsc Port_A,0
		;Bsf Ctrl,3

		Movwf W_Tmp	 	;Copy W to W_Tmp register
		Swapf STATUS,0 	;Swap status to be saved into W
		Clrf STATUS 	;bank 0, regardless of current bank, Clears IRP,RP1,RP0
		Movwf STA_Tmp 	;Save status to bank zero STA_Tmp register
		Movf PCLATH,0 	;Only required if using pages 1, 2 and/or 3
		Movwf PCL_Tmp 	;Save PCLATH into W
		Clrf PCLATH 	;Page zero, regardless of current page
;*******Salvataggio registri*******
		Btfss Intcon,2
		Goto Ext_Int
TMR_Int Bcf Intcon,2	;Azzera bit relativo all'interrupt TMR0
		Bsf Ctrl,5		;Bit controllo calcolo controllore PID
		Goto Int_Ex		;Uscita routine interrupt
Ext_Int	Bcf Intcon,1	;Azzera bit relativo all'interrupt esterna
		;Btfss Port_B,0
		;Goto ENCLA		;Testing
		;Btfsc Port_A,0	;Testing
		;Goto Int_Ex	;Testing
ENCLA	Btfss Port_A,0
		Goto Dir_D
		;Btfss Port_A,0	;Lettura sfasamento linea B encoder
		;Goto Dir_D
		;Incf enc,1		;Incrementa variabile lettura encoder
		Incf TmpEU,1
		Movlw 0x28 ;1C (28 impulsi-->1 incremento)
		Xorwf TmpEU,0
		Btfss STATUS,2
		Goto Int_Ex
		Clrf TmpEU
		Incf enc,1		;Incrementa variabile lettura encoder
Dir_D	Incf TmpED,1
		Movlw 0x29		;(28 impulsi-->1 decremento)
		Xorwf TmpED,0
		Btfss STATUS,2
		Goto Int_Ex
		Clrf TmpED
		Decf enc,1		;Decrementa variabile lettura encoder
;*******Ripristino registri*******
Int_Ex	Movf PCL_Tmp,0 	;Restore PCLATH
		Movwf PCLATH 	;Move W into PCLATH
		Swapf STA_Tmp,0 ;Swap STA_Tmp register into W
		Movwf STATUS 	;Move W into STATUS register
		Swapf W_Tmp,1 	;Swap W_Tmp
		Swapf W_Tmp,0 	;Swap W_Tmp into W
;*******Ripristino registri*******
		;Bsf Intcon,7	;Riabilita gestione interrupt
		Retfie
;*******Controllo TIMER*******
Init	Org 1000h
		Clrf TmpEU
		Clrf TmpED
		;*******TB Multiplier*******
;TBMul	;Clrf tev_h	;Impostazione angolo desiderato
;		Movlw 0xFF
;		Movwf tev_h
;		Movlw 0xC4	;Impostazione angolo desiderato
;		Movwf tev_l	;Impostazione angolo desiderato (180°)
		;Clrf enca0	;Impostazione posizione encoder
;		Movlw 0xFF
;		Movwf enca1
		;Clrf enca1	;Impostazione posizione encoder
;		Movlw 0xFF
;		Movwf enca2
		;Clrf enca2	;Impostazione posizione encoder
;		Movlw 0x3F
;		Movwf enca2
;		Movlw 0xEC
;		Movwf enca3
		;Clrf enca3	;Impostazione posizione encoder
		;Movlw 0x01
		;Movwf err_p0
		;Clrf err_p0	;Impostazione errore precedente
		;Clrf err_p1	;Impostazione errore precedente
		;Clrf err_p2	;Impostazione errore precedente
		;Clrf err_p3	;Impostazione errore precedente
		;Clrf Ind	;Impostazione costante Kp
		;Movlw 0x64	;Impostazione costante Kp
		;Call EWrite	;Impostazione costante Kp (100)
		;Incf Ind,1	;Impostazione costante Ki
		;Movlw 0x04	;Impostazione costante Ki
		;Call EWrite	;Impostazione costante Ki (4)
		;Incf Ind,1	;Impostazione costante Kd
		;Movlw 0x32	;Impostazione costante Kd
		;Call EWrite	;Impostazione costante Kd (50)
		;Clrf Ctrl
;		Call PID_Cal
;		Goto $-1
		;*******TB Multiplier*******
		Clrf Port_A
		Clrf Port_B
		Movlw 0x80
		Movwf e_ctrl
		Movwf enc
		Bsf STATUS,5
		Movlw B'00000110'	;Linee RAx digitali
		Movwf ADCON1
		Movlw B'00001001'
		Movwf TRISA
		Movlw B'00001111'
		Movwf TRISB
		Movlw B'10011001'	;Linea 2 controllo PWM
		Movwf TRISC
		Movlw B'11010111'	;1/256 TMR0 rate
		Movwf OPT	;Interrupt esterna sul fronte di salita
		Bcf STATUS,5
;*******Inizializzazione display*******
InDisp	Bcf Port_A,2
		;Bsf Port_C,5	;Testing
		Movlw 0x04	;(50->15,46 msec)
		Call Del2	;Attendi 15,46 msec @4 MHz
		;Bcf Port_C,5	;Testing
		Movlw 0x03	;Interfaccia a 8 bit
		Movwf Port_B
		Call Enable	;1
		Movlw 0x20	;(15->4 msec)
		Call Del1	;Attendi 5 msec @4 MHz
		;Movlw 0x03	;Interfaccia a 8 bit
		;Movwf Port_B
		Call Enable	;2
		Movlw 0x90	;(85->100 usec)
		Call Del0	;Attendi 100 usec @4 MHz
		;Movlw 0x03	;Interfaccia a 8 bit
		;Movwf Port_B
		Call Enable	;3
		Movlw 0x90	;(85->100 usec)
		Call Del0	;Attendi 100 usec @4 MHz
		Movlw 0x02	;Interfaccia a 4 bit (02)
		Movwf Port_B
		Call Enable	;4
		Movlw 0x90	;(85->100 usec)
		Call Del0	;Attendi 100 usec @4 MHz
		Movlw 0x82
		Call WrDisp	;Interfaccia a 4 bit - 2 righe
		Movlw 0x80	;Display Off
		Call WrDisp
		Movlw 0x10	;Display Clear
		Call WrDisp
		Movlw 0x60	;Entry mode set
		Call WrDisp
		;Movlw 0x08  ;Locazione 00h DD-RAM
		;Call WrDisp
		Movlw 0xC0	;Display On
		Call WrDisp	;Non visualizzare cursore
;*******Configurazione modulo PWM CCP1 e TMR2*******
		Bsf STATUS,5	;Passa al bank 1
		Movlw 0xFF		;PR2=255
		Movwf PR2		;FPWM=3.486KHz
		Clrf PIE1		;RIS=10bit@3.57MHz
		Bcf STATUS,5	;Ritorna al bank 0
		Clrf PIR1
		Movlw B'00001100'
		Movwf CCP1CON	;Imposta modalità PWM per il modulo CCP1
		Movlw B'00000000'
		Movwf T2CON		;Imposta prescaler TMR2 a 1
		;Bsf T2CON,2		;Abilita TMR2 (testing)
;*******Configurazione modulo PWM CCP1 e TMR2*******
		;Movlw B'10010000'	;Interrupt RB0 abilitato
		Movlw B'10110000'	;Interrupt TMR0+RB0 abilitati
		;Movlw B'10100000'	;Interrupt TMR0 abilitato
		;Movlw B'00110000'	;Interrupt TMR0+RB0 disabilitati
		Movwf Intcon
		;Ripristino posizione encoder
		Movlw 0x03
		Call ERead
		Movwf enca0
		Movlw 0x04
		Call ERead
		Movwf enca1
		Movlw 0x05
		Call ERead
		Movwf enca2
		Movlw 0x06
		Call ERead
		Movwf enca3
		Movlw 0x07
		Call ERead
		Movwf Ctrl

		Movlw 0x0B
		Call ERead
		Movwf tev_h
		Movlw 0x0C
		Call ERead
		Movwf tev_l
		;Ripristino posizione encoder
;*******Procedura di avvio*******
Start	Bsf Ctrl,6		;Testing
		Bcf Ctrl,1		;Controllo funzione pulsante Ok/Canc
		Call DspClr
		Movlw 0x18		;2a posizione 1a riga
		Call Riga
		Bsf PCLATH,3		;Pagina 3 (1800h-1FFFh)
		Call Disp			;Avvio sistema
Wait	Btfsc Ctrl,5	;Bit controllo calcolo controllore PID
		Call PID_Cal	;Calcola controllore PID e invia al PWM
		Call Enc_Ref	;Refresh variabili encoder incrementale
		Movlw 0x02
		Call Del1
		Btfsc Port_B,1
		Goto Menu
		Btfsc Port_B,2
		Goto Menu
		Btfsc Port_B,3
		Goto Start
		Decfsz Mn_Cnt,1
		Goto Wait
		Movlw 0xFF
		Movwf Mn_Cnt
		Btfss Ctrl,6
		Goto Start
		Bcf Ctrl,6
		Call PRiga
		Bsf PCLATH,3
		Call MME_Vis
		Movlw 0x1C
		Call Riga
		Bsf PCLATH,3
		Call Site
		Goto Wait
;*******Menu principale*******
Menu_Lp	Movlw 0xFF
		Movwf Mn_Cnt	;Variabile controllo uscita automatica menu
		Bcf Ctrl,1		;Controllo funzione pulsante Ok/Canc
Mn_Lp	Bcf T2CON,2		;Testing
		Call Enc_Ref	;Refresh variabili encoder incrementale
		Movlw 0xFF
		Call Del1
		Btfsc Port_B,1
		Goto DecMn
		Btfsc Port_B,2
		Goto IncMn
		Btfsc Port_B,3
		Goto Select
		Decfsz Mn_Cnt,1		;Variabile controllo uscita automatica menu
		Goto Mn_Lp			;Se diversa da 0 attendi pressione tasto
		Btfss MME,1			;Testing
		Goto Start			;Se 0 ritorna al punto di partenza
Menu	Call MnCom
		Bsf MME,0
		Call DspClr
		Goto IncMn
PID_Mn	Call ClrSLn
		Call MnCom
		Bsf MME,1
		Goto IncMn
Enc_Mn	Call ClrSLn
		Call MnCom
		Bsf MME,2
IncMn	Bcf STATUS,0	;Testing
		Rrf Aux,1
		;Btfss MME,1	;Controllo pressione tasto menu
		Goto $+3
DecMn	Bcf STATUS,0	;Testing
		Rlf Aux,1
		Btfss MME,2
		Goto StMn3
		;Btfsc MME,0
		;Goto StMn3
		;Btfsc MME,1
		;Goto StMn3
StMn2	Btfsc STATUS,0	;Carry bit
		Bsf Aux,1
		Bcf Aux,7
		Bcf STATUS,0
		Btfsc Aux,2
		Bsf Aux,0
		Bcf Aux,2	;Controllo limite superiore menu
		Btfsc MME,2
		Call PRiga
		Goto SlctOpt
StMn3	Btfsc STATUS,0	;Carry bit
		Bsf Aux,2
		Bcf Aux,7
		Bcf STATUS,0
		Btfsc Aux,3
		Bsf Aux,0
		Bcf Aux,3	;Controllo limite superiore menu
		Btfsc MME,0
		Call PRiga	;Posiziona sulla 1a riga
		Btfsc MME,1
		Call SRiga	;1a posizione 2a riga
		Goto SlctOpt
;StMn4	Btfsc STATUS,0	;Carry bit (menu3-->menu4)
;		Bsf Aux,3
;		Bcf Aux,7
;		Bcf STATUS,0
;		Btfsc Aux,4
;		Bsf Aux,0
;		Bcf Aux,4	;Controllo limite superiore menu
;		Call PRiga	;Posiziona sulla 1a riga
;		Goto SlctOpt
;StMn5	Btfsc STATUS,0		;Carry bit
;		Bsf Aux,4
;		Bcf Aux,7
;		Bcf STATUS,0
;		Btfsc Aux,5
;		Bsf Aux,0
;		Bcf Aux,5	;Controllo limite superiore menu
;		Goto SlctOpt
SlctOpt	Btfsc Aux,0	
		Call Aux0
		Btfsc Aux,1
		Call Aux1
		Btfsc Aux,2
		Call Aux2
		Btfsc Aux,3
		Call Aux3
		Btfsc Aux,4
		Call Aux4
		Goto Menu_Lp
Aux0	Bsf PCLATH,3	;Pagina 3 (1800h-1FFFh)
		Btfsc MME,0
		Call PID_Vs		;Visualizza "Impostazione PID"
		Btfsc MME,1
		Call PID_Vs		;Visualizza "Imposta kp:"
		Btfsc MME,2
		Call Res_Vs		;Visualizza "System reset!: "
		Return
Aux1	Bsf PCLATH,3	;Pagina 3 (1800h-1FFFh)
		Btfsc MME,0
		Call Teta_Vs		;Visualizza "Angolo desider."
		Btfsc MME,1
		Call PID_Vs		;Visualizza "Imposta ki:"
		Btfsc MME,2
		Call Enc_Rd		;Visualizza "Lettura encoder:"
		Return
Aux2	Bsf PCLATH,3	;Pagina 3 (1800h-1FFFh)
		Btfsc MME,0
		Call Enc_Vs		;Visualizza "Imposta encoder:"
		Btfsc MME,1
		Call PID_Vs		;Visualizza "Imposta kd:"
		Return
Aux3	;Bsf PCLATH,3	;Pagina 3 (1800h-1FFFh)
		Return
Aux4	;Bsf PCLATH,3	;Pagina 3 (1800h-1FFFh)
		Return
Select	Movlw 0xFF
		Call Del1
		Btfsc Aux,0
		Goto SAux0
		Btfsc Aux,1
		Goto SAux1
		Btfsc Aux,2
		Goto SAux2
		Btfsc Aux,3
		Goto SAux3
		Btfsc Aux,4
		Goto SAux4
		Btfsc Port_B,3
		Goto Start
SAux0	Btfsc MME,0
		Goto PID_Mn		;Menu impostazione PID
		Btfsc MME,1
		Goto P_Set	;Procedura settaggio valore kp
		Btfsc MME,2
		Goto Sys_Res	;Resetta lettura encoder
SAux1	Btfsc MME,0
		Goto Te_Set		;Procedura settaggio angolo desiderato
		Btfsc MME,1
		Goto ID_Set	;Procedura settaggio valore ki
		Btfsc MME,2
		Goto EncDis		;Visualizza lettura encoder
SAux2	Btfsc MME,0		;Lettura encoder
		Goto Enc_Mn		;Menu Encoder
		Btfsc MME,1
		Goto ID_Set	;Procedura settaggio valore kd
SAux3	;...
SAux4	;...
MnClr	Call DspClr		;Display Clear
MnCom	Clrf MME
		Clrf Aux
		Bsf Aux,0
		Return
Sys_Res	Clrf enca3		;Reset impostazioni sistema
		Clrf enca2
		Clrf enca1
		Clrf enca0
		Call SRiga
		Bsf PCLATH,3
		Call Exe
		Call Enc_Sv
		Goto Wait		
;*******Procedura lettura encoder (10.22bit + 10.22bit)*******
;*******Procedura refresh encoder*******
ResVar	Movlw 0x80
		Movwf enc
		Movwf e_ctrl
		Return	
Enc_Ref	Clrf ripp
		Clrf encb3
		Movlw B'01000001'	;Shift a dx 4 posizioni: 2.^-6
		Movwf encb2
		Clrf encb1
		Clrf encb0
		Movf e_ctrl,0
		Subwf enc,0
		Btfsc STATUS,2
		Goto ResVar		;Se uguali non eseguire operazioni
		Btfss STATUS,0	;se il risultato è positivo decrementa valore
		Goto DecVEnc	;se il risultato è negativo incrementa valore
IncVEnc	Btfsc Ctrl,4
		Goto DecNV
IncV	Bsf Port_C,5	;Testing
		Movf enca3,0	;Limite superiore (360°)
		Andlw 0x7F
		Xorlw 0x5A
		Btfss STATUS,2
		Goto IncEnc
		Clrf enca3
		Clrf enca2
		Clrf enca1		;Testing
		Clrf enca0		;Limite superiore (360°)
IncEnc	Btfss Ctrl,4
		Decf enc,1		;Variabile controllo encoder interrupt (test)
		Btfsc Ctrl,4
		Incf enc,1		;Variabile controllo encoder interrupt (test)
		Call Add32		;X+(2.^-6)
		Movlw 0x01
		Call ShRegs		;Shift a dx 4 posizioni: 2.^-7
		Call Add32		;X+(2.^-6+2.^-7)
		Movlw 0x07
		Call ShRegs		;Shift a dx 3 posizioni: 2.^-14
		Call Sub32		;X+(2.^-6+2.^-7-2.^-14)
		Movlw 0x01
		Call ShRegs		;Shift a dx 1 posizione: 2.^-15
		Call Sub32		;X+(2.^-6+2.^-7-2.^-14-2.^-15)
		Bcf Port_C,5	;Testing
		Return

		;Call Add32
		;Movlw 0x01
		;Call ShRegs		;Shift a dx 1 posizione: 2.^-11
		;Call Sub32		;X+(2.^-5+2.^-8+2.^-9-2.^-11-2.^-14)
		;Call Add32
		;Movlw 0x01
		;Call ShRegs		;Shift a dx 1 posizione: 2.^-12
		;Call Sub32		;X+(2.^-5+2.^-8+2.^-9-2.^-11-2.^-14-2.^-16)
		;Call Add32
		;Bcf Port_C,5	;Testing
		;Return
DecVEnc	Btfsc Ctrl,4
		Goto IncV
DecNV	Bsf Port_C,5	;Testing
		Movlw 0x00
		Xorwf enca3,0
		Btfss STATUS,2
		Goto DecV
		Movlw 0x00
		Xorwf enca2,0
		Btfss STATUS,2
		Goto DecV
		Movlw B'00010000'
		Xorwf Ctrl,1	;Bit segno variabile encoder
		Goto IncV
DecV	Btfss Ctrl,4	
		Incf enc,1		;Variabile controllo encoder interrupt (test)
		Btfsc Ctrl,4
		Decf enc,1		;Variabile controllo encoder interrupt (test)
		Call Sub32		;X+(-2.^-6)
		Movlw 0x01
		Call ShRegs		;Shift a dx 4 posizioni: 2.^-7
		Call Sub32		;X+(-2.^-6-2.^-7)
		Movlw 0x07
		Call ShRegs		;Shift a dx 3 posizioni: 2.^-14
		Call Add32		;X+(-2.^-6-2.^-7+2.^-14)
		Movlw 0x01
		Call ShRegs		;Shift a dx 1 posizione: 2.^-15
		Call Add32		;X+(-2.^-6-2.^-7+2.^-14+2.^-15)
		Bcf Port_C,5	;Testing
		Return

		;Call Sub32
		;Movlw 0x01
		;Call ShRegs		;Shift a dx 1 posizione: 2.^-11
		;Call Add32		;X+(-2.^-5-2.^-8-2.^-9+2.^-11+2.^-14)
		;Call Sub32
		;Movlw 0x01
		;Call ShRegs		;Shift a dx 1 posizione: 2.^-12
		;Call Add32		;X+(-2.^-5-2.^-8-2.^-9+2.^-11+2.^-14+2.^-16)
		;Call Sub32
		;Bcf Port_C,5	;Testing
		;Return
Add32	Bcf STATUS,0
		Movf ripp,0
		Addwf encb0,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp
		Addwf enca0,1
		Btfsc STATUS,0
		Incf ripp,1
		Movf encb1,0
		Addwf ripp,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp,1	
		Addwf enca1,1	
		Btfsc STATUS,0
		Incf ripp,1
		Movf encb2,0
		Addwf ripp,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp,1	
		Addwf enca2,1
		Btfsc STATUS,0
		Incf ripp,1
		Movf encb3,0
		Addwf ripp,0
		Clrf ripp
		;Btfsc STATUS,0
		;Incf ripp,1	
		Addwf enca3,1
		;Btfsc STATUS,0
		;Incf ripp,1
		Return
Sub32	Comf encb0,1		;Complemento a 2 operando
		Comf encb1,1
		Comf encb2,1
		Comf encb3,1
		Incf ripp,1
		Call Add32
		Comf encb0,1
		Comf encb1,1
		Comf encb2,1
		Comf encb3,1
		Return
ShRegs	Movwf CntEnc	;Numero di shift
ShReg	Bcf STATUS,0	;Azzera riporto
		Rrf encb3,1
		Rrf encb2,1
		Rrf encb1,1
		Rrf encb0,1
		Decfsz CntEnc,1	;Controllo shift
		Goto ShReg
		Return
;*******Moltiplicazione 40 bit (32bit x 8bit)*******
;opa0..3-->moltiplicando a 32 bit
;opa0..4-->risultato moltiplicazione a 40 bit
;opb0-->moltiplicatore a 8 bit
;tmp0..4-->prodotti parziali a 40 bit
;*******Testing*******
;MulTest	Movlw 0x63
;		Movwf opa3
;		Movlw 0xA7
;		Movwf opa2
		;Movwf opa1
		;Clrf opa3
		;Clrf opa2
		;Clrf opa1
;		Movlw 0x0D
;		Movwf opa1
;		Movlw 0x19
		;Movlw 0x00
;		Movwf opa0
		;Clrf opb3
		;Clrf opb2
		;Clrf opb1
		;Movlw 0x63
		;Movwf opb3
		;Movlw 0xA7
		;Movwf opb2
		;Movlw 0x0D
		;Movwf opb1
;		Movlw 0x64
;		Movwf opb0
;		Bcf Ctrl,0
;		Bcf Ctrl,2
;*******Testing*******
Mul64	Clrf opa4		;Setta numero positivo
		Clrf opa5
		Clrf opa6
		;Clrf opa7
		Clrf ripp
		Btfsc Ctrl,2
		Goto Mul40
		Bcf opb3,7
		Btfsc opb0,7
		Bsf opb3,7
Mul40	Movlw 0x08
		Btfsc Ctrl,2	;Bit cotrollo Mul56 o Mul40 bit
		Movlw 0x20		;(0x08 calcolo PID)
		Movwf Cnt		;Contatore 8 shift
		Btfss opa3,7	;Controllo segno moltiplicando
		Goto PMul64
		Movlw 0xFF		;Setta numero negativo
		Movwf opa4
		Movwf opa5
		Movwf opa6
		;Movwf opa7
PMul64	Call B64Set		;Imposta prodotto parziale
		Btfsc opb3,7	;Controllo segno moltiplicatore (test) (opb0)
		Call C2Oprt		;Complemento a 2 dei termini (test)
		Call ShR64		;Shift a dx moltiplicatore
		Btfsc STATUS,0	;Test bit moltiplicatore
		Goto Mul64Lp	;Esegui moltiplicazione
Blk64	Decf Cnt,1		;Testing
		Movlw 0x00
		Xorwf Cnt,0
		Btfsc STATUS,2
		Return
		Call ShL64		;Shift a sx prodotto parziale (PMul40)
		Call ShR64		;Shift a dx moltiplicatore		
		Btfss STATUS,0	;Salta somma zeri (moltiplicatore)
		Goto Blk64
		Call B64ISet	;Inizializza risultato
		;Goto Mul64Lp	;Esegui moltiplicazione
Mul64Lp	Call ShL64		;Shift a sx prodotto parziale
		Call ShR64		;Shift a dx moltiplicatore
		Btfsc STATUS,0	;Test bit moltiplicatore
		Call Add64M		;Procedura somma a 64 bit
		Decfsz Cnt,1
		Goto Mul64Lp	;Ripeti ciclo 8 volte
		Return
B64Set	Movf opa0,0		;Copia moltiplicando nella variabile tmp
		Movwf tmp0
		Movf opa1,0		;Copia moltiplicando nella variabile tmp
		Movwf tmp1
		Movf opa2,0		;Copia moltiplicando nella variabile tmp
		Movwf tmp2
		Movf opa3,0		;Copia moltiplicando nella variabile tmp
		Movwf tmp3
		Movf opa4,0		;Copia moltiplicando nella variabile tmp
		Movwf tmp4
		Movf opa5,0		;Copia moltiplicando nella variabile tmp
		Movwf tmp5
		Movf opa6,0
		Movwf tmp6
		;Movf opa7,0
		;Movwf tmp7
		Return
B64ISet	Movf tmp0,0		;Copia prodotto parziale nel risultato
		Movwf opa0
		Movf tmp1,0		;Copia prodotto parziale nel risultato
		Movwf opa1
		Movf tmp2,0		;Copia prodotto parziale nel risultato
		Movwf opa2
		Movf tmp3,0		;Copia prodotto parziale nel risultato
		Movwf opa3
		Movf tmp4,0		;Copia prodotto parziale nel risultato
		Movwf opa4
		Movf tmp5,0		;Copia prodotto parziale nel risultato
		Movwf opa5
		Movf tmp6,0		;Copia prodotto parziale nel risultato
		Movwf opa6
		;Movf tmp7,0		;Copia prodotto parziale nel risultato
		;Movwf opa7
		Return
ShL64	Bcf STATUS,0	;Azzera riporto
		Rlf tmp0,1		;Ruota a sx prodotto parziale
		Rlf tmp1,1		;Ruota a sx prodotto parziale
		Rlf tmp2,1		;Ruota a sx prodotto parziale
		Rlf tmp3,1		;Ruota a sx prodotto parziale
		Rlf tmp4,1		;Ruota a sx prodotto parziale
		Rlf tmp5,1		;Ruota a sx prodotto parziale
		Rlf tmp6,1		;Ruota a sx prodotto parziale
		;Rlf tmp7,1		;Ruota a sx prodotto parziale
		Return
ShR64	;Bcf STATUS,0
		;Btfsc opb3,7
		;Bsf STATUS,0
		Rrf opb3,1		;Ruota a dx moltiplicatore
		Rrf opb2,1		;Ruota a dx moltiplicatore
		Rrf opb1,1		;Ruota a dx moltiplicatore
		Rrf opb0,1		;Ruota a dx moltiplicatore
		Return
Add64M	Bcf STATUS,0
		Movf ripp,0
		Addwf tmp0,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp,1
		Addwf opa0,1
		Btfsc STATUS,0
		Incf ripp,1
		Movf tmp1,0
		Addwf ripp,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp,1
		Addwf opa1,1
		Btfsc STATUS,0
		Incf ripp,1
		Movf tmp2,0
		Addwf ripp,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp,1
		Addwf opa2,1
		Btfsc STATUS,0
		Incf ripp,1
		Movf tmp3,0
		Addwf ripp,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp,1
		Addwf opa3,1
		Btfsc STATUS,0
		Incf ripp,1
		Movf tmp4,0
		Addwf ripp,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp,1
		Addwf opa4,1
		Btfss Ctrl,2	;Skip somma 56 bit
		Clrf ripp		;Skip somma 56 bit
		Btfss Ctrl,2	;Skip somma 56 bit
		Return			;Skip somma 56 bit
		Btfsc STATUS,0
		Incf ripp,1
		Movf tmp5,0
		Addwf ripp,0
		Clrf ripp
		Btfsc STATUS,0
		Incf ripp,1
		Addwf opa5,1
		Btfsc STATUS,0
		Incf ripp,1
		Movf tmp6,0
		Addwf ripp,0
		Clrf ripp
		;Btfsc STATUS,0
		;Incf ripp,1
		Addwf opa6,1
		;Btfsc STATUS,0
		;Incf ripp,1
		Return

		;Btfsc STATUS,0
		;Incf ripp,1
		;Movf tmp6,0
		;Addwf ripp,0
		;Clrf ripp
		;Btfsc STATUS,0
		;Incf ripp,1
		;Addwf opa6,1
		;Btfsc STATUS,0
		;Incf ripp,1
		;Movf tmp7,0
		;Addwf ripp,0
		;Clrf ripp
		;Btfsc STATUS,0
		;Incf ripp,1
		;Addwf opa7,1
		;Btfsc STATUS,0
		;Incf ripp,1
		;Return
Sub64M	Comf tmp0,1		;Complemento a 2 operando
		Comf tmp1,1		;Sottrazione 40 bit
		Comf tmp2,1
		Comf tmp3,1
		Comf tmp4,1
		Comf tmp5,1
		Comf tmp6,1
		Incf ripp,1
		Call Add64M
		Comf tmp0,1
		Comf tmp1,1
		Comf tmp2,1
		Comf tmp3,1
		Comf tmp4,1
		Comf tmp5,1
		Comf tmp6,1
		Return
C2Oprt	Comf opa0,1	;*******Complemento a 2 moltiplicando*******
		Movlw 0x01
		Addwf opa0,1
		Comf opa1,1
		Movlw 0x01
		Btfsc STATUS,0
		Addwf opa1,1
		Comf opa2,1
		Btfsc STATUS,0
		Addwf opa2,1
		Comf opa3,1
		Btfsc STATUS,0
		Addwf opa3,1
		Comf opa4,1
		Btfsc STATUS,0
		Addwf opa4,1
		Comf opa5,1
		Btfsc STATUS,0
		Addwf opa5,1
		Comf opa6,1
		Btfsc STATUS,0
		Addwf opa6,1
		;Comf opa7,1
		;Btfsc STATUS,0
		;Addwf opa7,1
		Call B64Set	;Copia moltiplicando nella variabile tmp
		Comf opb0,1	;*******Complemento a 2 moltiplicatore*******
		Movlw 0x01
		Addwf opb0,1
		Comf opb1,1
		Movlw 0x01
		Btfsc STATUS,0
		Addwf opb1,1
		Comf opb2,1
		Btfsc STATUS,0
		Addwf opb2,1
		Comf opb3,1
		Btfsc STATUS,0
		Addwf opb3,1
		Return
;*******Procedura calcolo controllore PID*******
;*******Shift risultato di 22 posizioni a dx*******
ShR		Movwf Cnt
ShLpR	;Bsf STATUS,0
		;Btfss opa4,7	;(opa7,7)
		Bcf STATUS,0
		;Rrf opa7,1
		Rrf opa6,1
		Rrf opa5,1
		Rrf opa4,1
		Rrf opa3,1
		Rrf opa2,1
		Rrf opa1,1
		Rrf opa0,1
		Decfsz Cnt,1
		Goto ShLpR
		Return
;*******Shift risultato di 22 posizioni a dx*******
ShL		Movwf Cnt
ShLpL	Bcf STATUS,0
		Rlf opa0,1
		Rlf opa1,1
		Rlf opa2,1
		Rlf opa3,1
		Rlf opa4,1
		Rlf opa5,1
		Rlf opa6,1
		;Rlf opa7,1
		Decfsz Cnt,1
		Goto ShLpL
		Return
;*******Calcolo errore*******
PID_Cal	;Bcf T2CON,2		;Abilita TMR2
		Clrf ripp
		Movf enca0,0	;Sposta in W la posizione corrente
		Movwf tmp0
		Movf enca1,0	;Sposta in W la posizione corrente
		Movwf tmp1
		Movf enca2,0	;Sposta in W la posizione corrente
		Movwf tmp2
		Movf enca3,0	;Sposta in W la posizione corrente
		Movwf tmp3
		Clrf opa0		;Sposta in W l'angolo desiderato
		Clrf opa2		;Sposta in W l'angolo desiderato
		Bsf opa2,7
		Btfss tev_l,1
		Bcf opa2,7
		Bsf opa2,6
		Btfss tev_l,0
		Bcf opa2,6
		Movf tev_l,0	;Sposta in W l'angolo desiderato
		Movwf opa1
		Rrf opa1,1
		Rrf opa1,0
		Movwf opa3
		Bsf opa3,7
		Btfss tev_h,1
		Bcf opa3,7
		Bsf opa3,6
		Btfss tev_h,0
		Bcf opa3,6		;Sposta in W l'angolo desiderato
		Clrf opa1
		Btfsc Ctrl,4	;Controllo segno variabili encoder
		Call Add64M		;Calcola valore nuovo errore (somma a 32 bit)
		Btfss Ctrl,4	;Controllo segno variabili encoder
ErrSav	Call Sub64M		;Calcola valore nuovo errore (sottrazione a 32 bit)
		Movf opa0,0		;Salva nuovo valore dell'errore
		Movwf err_n0
		Movf opa1,0		;Salva nuovo valore dell'errore
		Movwf err_n1
		Movf opa2,0		;Salva nuovo valore dell'errore
		Movwf err_n2
		Movf opa3,0		;Salva nuovo valore dell'errore
		Movwf err_n3


		Movlw 0x00
		Xorwf err_n3,0
		Btfss STATUS,2
		Goto IntCal
		Movf err_n2,0
		Andlw 0xC0
		Xorlw 0x00
		Btfss STATUS,2
		Goto IntCal
		Btfsc Ctrl,7
		Return
		Decfsz PIDSk,1
		Goto IntCal
		Bsf Ctrl,7
		Bcf T2CON,2		;Disabilita TMR2
		Return

		;Movf opa4,0	;Testing
		;Movwf err_n4	;Testing
;*******Calcolo errore*******
;*******Calcolo termine integrale*******
;*******Calcolo sommatoria errore (termine integrale)*******
IntCal	Bcf Ctrl,7		;Testing
		Movf err_p0,0	;Trasferisci errore precedente
		Movwf tmp0
		Movf err_p1,0	;Trasferisci errore precedente
		Movwf tmp1
		Movf err_p2,0	;Trasferisci errore precedente
		Movwf tmp2
		Movf err_p3,0	;Trasferisci errore precedente
		Movwf tmp3
		Call Add64M		;Calcola somma a 32 bit
;*******Calcolo sommatoria errore (termine integrale)*******
		Movlw 0x01
		Call ERead
		Movwf opb0	;Trasferisci costante ki nel moltiplicatore
		Call Mul64	;Esegui moltiplicazione (32x8 bit)
		Movlw 0x06
		Call ShR	;Dividi per Fs (=64)
		Movf opa0,0	;Salva risultato controllore integrale
		Movwf ki_r0
		Movf opa1,0	;Salva risultato controllore integrale
		Movwf ki_r1
		Movf opa2,0	;Salva risultato controllore integrale
		Movwf ki_r2
		Movf opa3,0	;Salva risultato controllore integrale
		Movwf ki_r3
		Movf opa4,0	;Salva risultato controllore integrale
		Movwf ki_r4
		;Movf opa5,0
		;Movwf ki_r5
;*******Calcolo termine integrale*******
;*******Calcolo termine proporzionale*******
Pro_Cal	Movf err_n0,0	;Trasferisci errore nel moltiplicando
		Movwf opa0
		Movf err_n1,0	;Trasferisci errore nel moltiplicando
		Movwf opa1
		Movf err_n2,0	;Trasferisci errore nel moltiplicando
		Movwf opa2
		Movf err_n3,0	;Trasferisci errore nel moltiplicando
		Movwf opa3
		Clrw
		Call ERead
		Movwf opb0	;Trasferisci costante kp nel moltiplicatore
		Call Mul64	;Esegui moltiplicazione (32x8 bit)
		Movf opa0,0	;Salva risultato controllore proporzionale
		Movwf kp_r0
		Movf opa1,0	;Salva risultato controllore proporzionale
		Movwf kp_r1
		Movf opa2,0	;Salva risultato controllore proporzionale
		Movwf kp_r2
		Movf opa3,0	;Salva risultato controllore proporzionale
		Movwf kp_r3
		Movf opa4,0	;Salva risultato controllore proporzionale
		Movwf kp_r4
		;Movf opa5,0	;Salva risultato controllore proporzionale
		;Movwf kp_r5
		;Movf opa6,0	;Salva risultato controllore proporzionale
		;Movwf kp_r6
		;Movf opa7,0	;Salva risultato controllore proporzionale
		;Movwf kp_r7
;*******Calcolo termine derivativo*******
;*******Calcolo differenza errore [E(n)-E(n-1)]*******
		Movf err_n0,0 ;Trasferisci errore attuale nel 1mo addendo
		Movwf opa0
		Movf err_n1,0 ;Trasferisci errore attuale nel 1mo addendo
		Movwf opa1
		Movf err_n2,0 ;Trasferisci errore attuale nel 1mo addendo
		Movwf opa2
		Movf err_n3,0 ;Trasferisci errore attuale nel 1mo addendo
		Movwf opa3
		Movf err_p0,0 ;Trasferisci errore precedente nel 2mo addendo
		Movwf tmp0
		Movf err_p1,0 ;Trasferisci errore precedente nel 2mo addendo
		Movwf tmp1
		Movf err_p2,0 ;Trasferisci errore precedente nel 2mo addendo
		Movwf tmp2
		Movf err_p3,0 ;Trasferisci errore precedente nel 2mo addendo
		Movwf tmp3
		Call Sub64M	;Esegui somma (sottrazione a 32 bit)
;*******Calcolo differenza errore [E(n)-E(n-1)]*******
		Movlw 0x02
		Call ERead
		Movwf opb0	;Trasferisci costante kd nel moltiplicatore
		Call Mul64	;Esegui moltiplicazione
		Movlw 0x06	;Moltiplica per Fs (=64)
		Call ShL
;*******Somma tutti i contributi*******
		Movf ki_r0,0 ;Trasferisci termine integrale nell'operando
		Movwf tmp0
		Movf ki_r1,0 ;Trasferisci termine integrale nell'operando
		Movwf tmp1
		Movf ki_r2,0 ;Trasferisci termine integrale nell'operando
		Movwf tmp2
		Movf ki_r3,0 ;Trasferisci termine integrale nell'operando
		Movwf tmp3
		Movf ki_r4,0 ;Trasferisci termine integrale nell'operando
		Movwf tmp4
		;Movf ki_r5,0 ;Trasferisci termine integrale nell'operando
		;Movwf tmp5
		;Movf ki_r6,0 ;Trasferisci termine integrale nell'operando
		;Movwf tmp6
		;Movf ki_r7,0 ;Trasferisci termine integrale nell'operando
		;Movwf tmp7
		Call Add64M	;Esegui somma a 40 bit
		Movf kp_r0,0 ;Trasferisci termine proporzionale nell'operando
		Movwf tmp0
		Movf kp_r1,0 ;Trasferisci termine proporzionale nell'operando
		Movwf tmp1
		Movf kp_r2,0 ;Trasferisci termine proporzionale nell'operando
		Movwf tmp2
		Movf kp_r3,0 ;Trasferisci termine proporzionale nell'operando
		Movwf tmp3
		Movf kp_r4,0 ;Trasferisci termine proporzionale nell'operando
		Movwf tmp4
		;Movf kp_r5,0 ;Trasferisci termine proporzionale nell'operando
		;Movwf tmp5
		;Movf kp_r6,0 ;Trasferisci termine proporzionale nell'operando
		;Movwf tmp6
		;Movf kp_r7,0 ;Trasferisci termine proporzionale nell'operando
		;Movwf tmp7
		Call Add64M	;Esegui somma a 40 bit
;*******Somma tutti i contributi*******
;*******Salva errore errore attuale*******
		Movf err_n0,0 ;Salva errore attuale come errore precedente
		Movwf err_p0
		Movf err_n1,0 ;Salva errore attuale come errore precedente
		Movwf err_p1
		Movf err_n2,0 ;Salva errore attuale come errore precedente
		Movwf err_p2
		Movf err_n3,0 ;Salva errore attuale come errore precedente
		Movwf err_p3
;*******Salva errore errore attuale*******
;Il risultato definisce il duty cicle del controllore PWM
;*******Procedura gestione modulo PWM (CCP1 - linea RC2)*******
PWM_Set	;Bsf CCP1CON,5
		;Bsf CCP1CON,4
		;Btfss opa3,1	;Imposta bit LSB duty cicle (9^mo bit)
		;Bcf CCP1CON,5	;Imposta bit LSB duty cicle (9^mo bit)
		;Btfss opa3,0	;Imposta bit LSB duty cicle (10^mo bit)
		;Bcf CCP1CON,4	;Imposta bit LSB duty cicle (10^mo bit)
		;Bcf STATUS,0
		;Rrf opa3,0
		;Movwf Tmp
		;Bcf STATUS,0
		;Rrf Tmp,0
		;Movwf CCPR1L	;Imposta duty cicle controllo PWM
		
;*******Procedura disabilitazione modulo PWM errore nullo
;		Bsf CCP1CON,5
;		Bsf CCP1CON,4
;		Btfss opa2,7	;Imposta bit LSB duty cicle (9^mo bit)
;		Bcf CCP1CON,5	;Imposta bit LSB duty cicle (9^mo bit)
;		Btfss opa2,6	;Imposta bit LSB duty cicle (10^mo bit)
;		Bcf CCP1CON,4	;Imposta bit LSB duty cicle (10^mo bit)
;		Movf opa3,0
;		Andlw 0x3E
;		Movwf CCPR1L	;Imposta duty cicle controllo PWM


		;Bsf CCPR1L,5
		;Btfss opa4,0
		;Bcf CCPR1L,5

		;Bsf CCPR1L,7	;MSB risultato somma di tutti i contributi
		;Bsf CCPR1L,6	;MSB risultato somma di tutti i contributi
		;Btfss opa4,1	;MSB risultato somma di tutti i contributi
		;Bcf CCPR1L,7	;MSB risultato somma di tutti i contributi
		;Btfss opa4,0	;MSB risultato somma di tutti i contributi
		;Bcf CCPR1L,6	;MSB risultato somma di tutti i contributi
		Btfsc err_n3,7	;Valuta segno errore
		Bsf Port_A,1	;Direzione oraria
		Btfss err_n3,7
		Bcf Port_A,1	;Direzione antioraria
		Bsf T2CON,2		;Abilita TMR2
		;Bcf Ctrl,5		;Testing
		;Salvataggio posizione encoder
Enc_Sv	Movlw 0x03
		Movwf Ind
		Movf enca0,0
		Call EWrite
		Incf Ind,1
		Movf enca1,0
		Call EWrite
		Incf Ind,1
		Movf enca2,0
		Call EWrite
		Incf Ind,1
		Movf enca3,0
		Call EWrite
		Incf Ind,1
		Movf Ctrl,0
		Call EWrite
		;Salvataggio posizione encoder
		Return
;*******Procedura visualizzazione valore encoder display*******
EncDis	;*******Testing*******
		;Movlw 0x00
		;Movwf enca3
		;Movlw 0x02
		;Movwf enca2
		;Movlw 0x56
		;Movwf enca1
		;Movlw 0xC0
		;Movwf enca0
		;*******Testing*******
		Movlw 0x3C
		Call Riga		;4a posizione 2a riga
		Movlw 0xB2		;(2Bh '+')
		Btfsc Ctrl,4
		Movlw 0xD2		;(2Dh '-')
		Call WrDisp
		Call IntBCD	;Converti parte intera in BCD
		Bsf Ctrl,2		;Bit controllo moltiplicazione 40/56 bit
		Call FraBCD	;Converti parte frazionaria in BCD
		Bcf Ctrl,2		;Bit controllo moltiplicazione 40/56 bit
		;Bsf STATUS,5	;Passa al bank 1
		;Movf RI0,0		;Visualizza nibble superiore
		;Bcf STATUS,5	;Ritorna al bank 0
		;Andlw 0xF0
		;Xorlw 0x00
		;Bcf Ctrl,6		;Testing
		;Btfsc STATUS,2
		;Bsf Ctrl,6		;Testing
		;Btfss Ctrl,6	;Testing		
		;Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Swapf RI0,0		;Visualizza nibble inferiore
		Bcf STATUS,5	;Ritorna al bank 0
		Andlw 0xF0
		Xorlw 0x00
		Bcf Ctrl,6		;Testing
		Btfsc STATUS,2
		Bsf Ctrl,6		;Testing
		Btfss Ctrl,6	;Testing
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Movf RI1,0		;Visualizza nibble superiore
		Bcf STATUS,5	;Ritorna al bank 0
		Btfss Ctrl,6	;Testing
		Goto $+4
		Andlw 0xF0
		Xorlw 0x00
		Btfss STATUS,2
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Swapf RI1,0		;Visualizza nibble inferiore
		Bcf STATUS,5	;Ritorna al bank 0
		Call VarVis		;Visualizza cifra
		Movlw 0xE2		;(2Eh .)
		Call WrDisp
		Bsf STATUS,5	;Passa al bank 1
		Movf RF2,0		;Visualizza nibble superiore
		Bcf STATUS,5	;Ritorna al bank 0
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Swapf RF2,0		;Visualizza nibble inferiore
		Bcf STATUS,5	;Ritorna al bank 0
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Movf RF1,0		;Visualizza nibble superiore
		Bcf STATUS,5	;Ritorna al bank 0
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Swapf RF1,0		;Visualizza nibble inferiore
		Bcf STATUS,5	;Ritorna al bank 0
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Movf RF0,0		;Visualizza nibble superiore
		Bcf STATUS,5	;Ritorna al bank 0
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Swapf RF0,0		;Visualizza nibble inferiore
		Bcf STATUS,5	;Ritorna al bank 0
		Call VarVis		;Visualizza cifra
		Goto Wait
;*******Procedura conversione parte frazionaria-->codice BCD/ASCII*******
FraBCD	Clrf opb3		;Moltiplica per 10.^6
		Movlw 0x0F
		Movwf opb2
		Movlw 0x42
		Movwf opb1
		Movlw 0x40
		Movwf opb0
		Movf enca0,0	;Carica valore encoder nel moltiplicando
		Movwf opa0
		Movf enca1,0
		Movwf opa1
		Movf enca2,0
		Movwf opa2
		Movf enca3,0
		Movwf opa3
		Call Mul64		;Esegui moltiplicazione *10.^6
		Movlw 0x16		;Esegui shift a dx di 22 posizioni
		Call ShR		;Testing (Salva parte intera)
		Call Div_10	;Dividi per 10
		Movf opb3,0		;Salva resto
		Bsf STATUS,5	;Passa al bank 1
		Movwf RF0
		Bcf STATUS,5	;Ritorna al bank 0
		Call Div_10	;Dividi per 10
		Swapf opb3,0	;Salva resto
		Bsf STATUS,5	;Passa al bank 1
		Xorwf RF0,1
		Bcf STATUS,5	;Ritorna al bank 0
		Call Div_10	;Dividi per 10
		Movf opb3,0		;Salva resto
		Bsf STATUS,5	;Passa al bank 1
		Movwf RF1
		Bcf STATUS,5	;Ritorna al bank 0
		Call Div_10	;Dividi per 10
		Swapf opb3,0	;Salva resto
		Bsf STATUS,5	;Passa al bank 1
		Xorwf RF1,1
		Bcf STATUS,5	;Ritorna al bank 0
		Call Div_10	;Dividi per 10
		Movf opb3,0		;Salva resto
		Bsf STATUS,5	;Passa al bank 1
		Movwf RF2
		Bcf STATUS,5	;Ritorna al bank 0
		Call Div_10	;Dividi per 10
		Swapf opb3,0	;Salva resto
		Bsf STATUS,5	;Passa al bank 1
		Xorwf RF2,1
		Bcf STATUS,5	;Ritorna al bank 0
		Return
;*******Procedura conversione parte intera-->codice BCD/ASCII*******
;Numero binario da convertire in byte1 (parte alta), byte0 (parte bassa)
;Risultato BCD in RI0 (parte alta), RI1 (parte bassa)
;*******Testing*******
;B2BTest	;Movlw 0FF
		;Bsf STATUS,5
		;Movlw 0x63
 		;Movwf byte3
		;Movlw 0xA7
		;Movwf byte2
		;Movlw 0x0D
		;Movwf byte1 ; The 16 bit binary number = FFFF
		;Movlw 0x19
		;Movwf byte0
;*******Testing*******
IntBCD	Movf enca3,0
		Bsf STATUS,5
		Movwf byte0
		Clrf byte1
		Bcf STATUS,0
		Rlf byte0,1
		Btfsc STATUS,0
		Bsf byte1,1
		Bcf STATUS,0
		Rlf byte0,1
		Btfsc STATUS,0
		Bsf byte1,0
		Bcf STATUS,5
		Movf enca2,0
		Bsf STATUS,5
		Movwf RI0
		Btfsc RI0,7
		Bsf byte0,1
		Btfsc RI0,6
		Bsf byte0,0
B2BCD	Bcf STATUS,0 ; clear the carry bit
		Movlw 0x10
 		Movwf count
 		Clrf RI0
 		Clrf RI1
		Btfss byte1,1	;Testing
		Goto Loop16
		Comf byte0,1		;Complemento a 2 parte intera
		Movlw 0x01
		Addwf byte0,1
		Comf byte1,1
		Movlw 0x01
		Btfsc STATUS,0
		Addwf byte1,1
		Movlw 0x03
		Andwf byte1,1
Loop16 	Rlf byte0,1
 		Rlf byte1,1
		;Rlf byte2,1
		;Rlf byte3,1
		;Rlf RI4,1
		;Rlf RI3,1
		;Rlf RI2,1
 		Rlf RI1,1
 		Rlf RI0,1
 		Decfsz count,1
 		Goto AdjDEC
		Bcf STATUS,5	;Ritorna al bank 0
 		Retlw 0
AdjDEC 	;Movlw RI4
 		;Movwf FSR
 		;Call AdjBCD
		;Movlw RI3
 		;Movwf FSR
 		;Call AdjBCD
		;Movlw RI2
 		;Movwf FSR
 		;Call AdjBCD
 		Movlw RI1
		Movwf FSR
		Call AdjBCD
		Movlw RI0
 		Movwf FSR
 		Call AdjBCD
 		Goto Loop16
AdjBCD 	Movlw 0x03
 		Addwf 0,0	;(0-->INDF)
 		Movwf temp
 		Btfsc temp,3 ; test if result > 7
 		Movwf 0
 		Movlw 30
 		Addwf 0,0
 		Movwf temp
 		Btfsc temp,7 ; test if result > 7
 		Movwf 0 ; save as MSD
 		Retlw 0
;*******Divisione 32 bit / 8 bit*******
;Dividendo in opa3..opa0
;Divisore in opb0
;Quoziente in opa3..opa0
;Resto in opb3
;*******Testing*******
;DivTest	Movlw 0xB5
;		Movwf opa0
;		Movlw 0x9B
;		Movwf opa1
;		Clrf opa2
;		Clrf opa3
;*******Testing*******
Div_10	Movlw 0x0A
		Movwf opb0
Div32_8
		Movlw 0x80
		Subwf opb0,0
		Movlw 0x08
		Btfsc STATUS,0
		Movwf opb2
		Movlw 0x40
		Subwf opb0,0
		Movlw 0x07
		Btfsc STATUS,0
		Movwf opb2
		Movlw 0x20
		Subwf opb0,0
		Movlw 0x06
		Btfsc STATUS,0
		Movwf opb2
		Movlw 0x10
		Subwf opb0,0
		Movlw 0x05
		Btfsc STATUS,0
		Movwf opb2
		Movlw 0x08
		Subwf opb0,0
		Movlw 0x04
		Btfsc STATUS,0
		Movwf opb2
		Movf opa0,0		;Copia variabili
		Movwf tmp0
		Movf opa1,0
		Movwf tmp1
		Movf opa2,0
		Movwf tmp2
		Movf opa3,0
		Movwf tmp3
		Movf opb2,0
		Movwf opb1		;Variabile controllo termine divisione
		Clrf opa3
		Clrf opa2
		Clrf opa1
		Clrf opa0
		Clrf opb3		;Azzera divisore parziale
DivShR	Movlw 0x21
		Xorwf opb1,0
		Btfsc STATUS,2
		Return			;Fine divisione
		Rlf tmp0,1
		Rlf tmp1,1
		Rlf tmp2,1
		Rlf tmp3,1
		Rlf opb3,1		;Divisore parziale
		Decfsz opb2,1
		Goto DivShR
Div32Lp Movf opb0,0
		Subwf opb3,0
		Btfss STATUS,2
		Goto DifNeg
		Clrf opb3		;Azzera divisore parziale
		Bsf STATUS,0 	;Divisore parziale e divisore uguali
		Call DivRot
		Incf opb2,1
		Incf opb1,1
		Goto DivShR
DifNeg	Btfsc STATUS,0
		Goto DifPos
		Call DivRot
		Incf opb2,1
		Incf opb1,1
		Goto DivShR
DifPos	Call DivRot
		Movwf opb3		;Memorizza resto parziale
		Incf opb2,1
		Incf opb1,1
		Goto DivShR
DivRot	Rlf opa0,1
		Rlf opa1,1
		Rlf opa2,1
		Rlf opa3,1
		Return
;*******Procedura impostazione PID(menu)*******
P_Set	Clrw			;Locazione EEPROM controllore proporzionale
		Call ERead		;Lettura EEPROM
		Movwf K			;Scrivi valore nella variabile costante controllore
		;Testing
		Movf tev_h,0	;Testing
		Movwf tmp5		;Testing
		Movf tev_l,0	;Testing
		Movwf tmp4		;Testing
		Btfss tev_h,7
		Goto P_Vis
		Comf tev_l,1		;Complemento a 2 parte intera
		Movlw 0x01
		Addwf tev_l,1
		Comf tev_h,1
		Movlw 0x01
		Btfsc STATUS,0
		Addwf tev_h,1
		;Movlw 0x03
		;Andwf byte1,1
		;Testing
		Goto P_Vis		;Visualizza valore sul display
P_Lp	Movlw 0xFF
		Movwf Mn_Cnt	;Variabile controllo uscita automatica menu
P_Loop	Call Enc_Ref	;Refresh variabili encoder incrementale
		Movlw 0xFF
		Call Del1
		Btfsc Port_B,2
		Goto P_Inc
		Btfsc Port_B,1
		Goto P_Dec
		Btfsc Port_B,3
		Goto P_Ex
		Decfsz Mn_Cnt,1	;Variabile controllo uscita automatica menu
		Goto P_Loop
P_Ex	Movlw 0x02
		Call Del2
		Btfsc Ctrl,1	;Se Ctrl,1 è 1 ritorna
		Goto P_Exit		;Ritorna nel punto iniziale
		Bsf Ctrl,1		;Bit controllo uscita menu
		Movf tmp5,0	;Testing
		Movwf tev_h	;Testing
		Movf tmp4,0	;Testing
		Movwf tev_l	;Testing
		Clrf Ind		;Locazioni EEPROM controllore proporzionale
		Movf K,0		;Salva variabile calcolo controllore
		Call EWrite		;Scrittura EEPROM
		Goto P_Lp
P_Exit	Movlw 0x01
		Movwf MME
		Movwf Aux		;Testing
		Call ClrSLn
		Goto Menu_Lp
P_Inc	Btfsc tev_h,0	;Controllo centinaia angolo desiderato
		Goto CU5
		Movlw 0x01		;teta=1
		Xorwf tev_l,0
		Btfss STATUS,2
		Goto CU10
		Movlw 0xFF	;Kp<=255
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU10	Movlw 0x0A		;(CU10)
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<10
		Goto CU15		;teta>9
		Movlw 0xBE	;Kp<=190
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU15	Movlw 0x0F
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<15
		Goto CU25		;teta>14
		Movlw 0x78	;Kp<=120
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU25	Movlw 0x19
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<25
		Goto CU50		;teta>24
		Movlw 0x46	;Kp<=70
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU50	Movlw 0x32
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<50
		Goto CU75		;teta>49
		Movlw 0x23	;Kp<=35
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU75	Movlw 0x4B
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<75
		Goto CU100		;teta>74
		Movlw 0x18	;Kp<=24
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU100	Movlw 0x64
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<100
		Goto CU150		;teta>99
		Movlw 0x12	;Kp<=18
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU150	Movlw 0x96
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<150
		Goto CU225		;teta>149
		Movlw 0x0C	;Kp<=12
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU225	Movlw 0xE1
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<225
		Goto CU5		;teta>224
		Movlw 0x08	;Kp<=8
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Inc
CU5		Movlw 0x05
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
Kp_Inc	Incf K,1
		Goto P_Vis
P_Dec	Btfsc tev_h,0	;Controllo centinaia angolo desiderato
		Goto CD1
		Movlw 0x01		;teta=1
		Xorwf tev_l,0
		Btfss STATUS,2
		Goto CD10
		Movlw 0x8C	;Kp>=140
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Dec		
CD10	Movlw 0x0A		;(CD10)
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<10
		Goto CD15		;teta>9 
		Movlw 0x46	;Kp>=70
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Dec
CD15	Movlw 0x0F
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<15
		Goto CD25		;teta>14
		Movlw 0x0F	;Kp>=15
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Dec
CD25	Movlw 0x19
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<25
		Goto CD50		;teta>24
		Movlw 0x0A	;Kp>=15
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Dec
CD50	Movlw 0x32
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<50
		Goto CD75		;teta>49
		Movlw 0x06	;Kp>=6
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Dec
CD75	Movlw 0x4B
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<75
		Goto CD1		;teta>74
		Movlw 0x03	;Kp>=3
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp
		Goto Kp_Dec
CD1		Movlw 0x01		;teta>74
		Xorwf K,0
		Btfsc STATUS,2
		Goto P_Lp	
Kp_Dec	Decf K,1
		Goto P_Vis
P_Vis	Movlw 0xBC
		Call Riga		;12a posizione 2a riga
		Bsf STATUS,5
		Clrf byte1
		Bcf STATUS,5
		Movf K,0
		Call ByteVs
		Movlw 0x02
		Call WrDisp
		Movlw 0x02
		Call WrDisp
		Goto P_Lp
ByteVs	Bsf STATUS,5
		Movwf byte0
		Call B2BCD
		Bsf STATUS,5	;Passa al bank 1
		Movf RI0,0		;Visualizza nibble superiore
		Bcf STATUS,5	;Ritorna al bank 0
		Andlw 0xF0
		Xorlw 0x00
		Btfss STATUS,2
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Swapf RI0,0		;Visualizza nibble inferiore
		Bcf STATUS,5	;Ritorna al bank 0
		Andlw 0xF0
		Xorlw 0x00
		Btfsc STATUS,2
		Goto ZSk
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Movf RI1,0		;Visualizza nibble superiore
		Bcf STATUS,5	;Ritorna al bank 0
		Goto ZNSk
ZSk		Bsf STATUS,5	;Passa al bank 1
		Movf RI1,0		;Visualizza nibble superiore
		Bcf STATUS,5	;Ritorna al bank 0
		Andlw 0xF0
		Xorlw 0x00
		Btfss STATUS,2
ZNSk	Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Swapf RI1,0		;Visualizza nibble inferiore
		Bcf STATUS,5	;Ritorna al bank 0
VarVis	Movwf Tmp	;Visualizza cifra
VarVs	Bsf Tmp,0	;Visualizza cifra
		Bsf Tmp,1	;Visualizza cifra
		Bcf Tmp,2	;Visualizza cifra
		Bcf Tmp,3	;Visualizza cifra
		Movf Tmp,0	;Visualizza cifra
		Goto WrDisp	;Visualizza cifra
;*******Procedura impostazione costanti Ki e Kp*******
ID_Set	Btfsc Aux,1		;Locazioni EEPROM controllore integrale
		Movlw 0x01
		Btfsc Aux,2		;Locazioni EEPROM controllore derivativo
		Movlw 0x02
		Call ERead		;Lettura EEPROM
		Movwf K			;Scrivi valore nella variabile costante controllore
		Goto ID_Vis		;Visualizza valore sul display
ID_Lp	Movlw 0xFF
		Movwf Mn_Cnt	;Variabile controllo uscita automatica menu
ID_Loop	Call Enc_Ref	;Refresh variabili encoder incrementale
		Movlw 0xFF
		Call Del1
		Btfsc Port_B,2
		Goto ID_Inc
		Btfsc Port_B,1
		Goto ID_Dec
		Btfsc Port_B,3
		Goto ID_Ex
		Decfsz Mn_Cnt,1	;Variabile controllo uscita automatica menu
		Goto ID_Loop
ID_Ex	Movlw 0x02
		Call Del2
		Btfsc Ctrl,1	;Se Ctrl,1 è 1 ritorna
		Goto ID_Exit		;Ritorna nel punto iniziale
		Bsf Ctrl,1		;Bit controllo uscita menu
		Btfsc Aux,1		;Locazioni EEPROM controllore integrale
		Movlw 0x01
		Btfsc Aux,2		;Locazioni EEPROM controllore derivativo
		Movlw 0x02
		Movwf Ind
		Movf K,0		;Salva valore costante
		Call EWrite		;Scrittura EEPROM
		Goto ID_Lp
ID_Exit	Movlw 0x01
		Movwf MME
		Movwf Aux		;Testing
		Call ClrSLn
		Goto Menu_Lp
ID_Inc	Movlw 0x06	;Ki<6
		Btfsc Aux,2
		Movlw 0x46	;Kd<70
		Xorwf K,0
		Btfsc STATUS,2
		Goto ID_Lp
		Incf K,1
		Goto ID_Vis
ID_Dec	Movlw 0x00
		Xorwf K,0
		Btfsc STATUS,2
		Goto ID_Lp
		Decf K,1
ID_Vis	Movlw 0xBC
		Call Riga		;12a posizione 2a riga
		Movf K,0
		Bsf STATUS,5
		Movwf byte0
		Clrf byte1		;Testing
		Call B2BCD
		Bsf STATUS,5	;Passa al bank 1
		Movf RI1,0		;Visualizza nibble superiore
		Bcf STATUS,5	;Ritorna al bank 0
		Andlw 0xF0
		Xorlw 0x00
		Btfss STATUS,2
		Call VarVis		;Visualizza cifra
		Bsf STATUS,5	;Passa al bank 1
		Swapf RI1,0		;Visualizza nibble inferiore
		Bcf STATUS,5	;Ritorna al bank 0
		Call VarVis		;Visualizza cifra
		Movlw 0x02		;(20h Space)
		Call WrDisp
		Goto ID_Lp
;*******Procedura settaggio angolo desiderato*******
Te_Set	;Movlw 0x0B
		;Call ERead
		;Movwf tev_h
		;Movlw 0x0C
		;Call ERead
		;Movwf tev_l
		Call ClrSLn
		Goto Te_Vis
Te_Lp	Movlw 0xFF
		Movwf Mn_Cnt	;Variabile controllo uscita automatica menu
Te_Loop	Call Enc_Ref	;Refresh variabili encoder incrementale
		Movlw 0xFF
		Call Del1
		Btfsc Port_B,2
		Goto Te_iv
		Btfsc Port_B,1
		Goto Te_dv
		Btfsc Port_B,3
		Goto Te_Ex
		Decfsz Mn_Cnt,1	;Variabile controllo uscita automatica menu
		Goto Te_Loop
Te_Ex	Movlw 0x02
		Call Del2
		Btfsc Ctrl,1
		Goto Te_Exit
		Bsf Ctrl,1
		Movlw 0x0B
		Movwf Ind
		Movf tev_h,0
		Call EWrite
		Incf Ind,1
		Movf tev_l,0
		Call EWrite
		Call TeVsKp		;Testing
		Movf tmp5,0	;Testing
		Movwf tev_h	;Testing
		Movf tmp4,0	;Testing
		Movwf tev_l	;Testing
		Goto Te_Lp
Te_Exit	Movlw 0x01
		Movwf MME
		Movlw 0x02		;Testing
		Movwf Aux		;Testing
		Call ClrSLn
		Goto Menu_Lp
Te_Vis	Movlw 0x6C
		Call Riga		;7a posizione 2a riga
		Movlw 0xB2		;(2Bh '+')
		Btfsc tev_h,7
		Movlw 0xD2		;(2Dh '-')
		Call WrDisp
		Movf tev_h,0
		Bsf STATUS,5	;Testing
		Movwf byte1		;Testing
		Bcf STATUS,5	;Testing
		Movf tev_l,0	;Testing
		Call ByteVs		;Testing
		Movlw 0xFD		;(DFh '°')
		Call WrDisp
		Movlw 0x02		;(20h Space)
		Call WrDisp
		Goto Te_Lp		;Testing
Te_iv	Movlw 0x01
		Xorwf tev_h,0
		Btfss STATUS,2
		Goto Te_i
		Movlw 0x67		;Controllo limite superiore
		Xorwf tev_l,0
		Btfss STATUS,2
		Goto Te_i
		;Clrf tev_h		;Impostazione ciclica
		;Clrf tev_l		;Impostazione ciclica
		Goto Te_Vis
Te_i	Movlw 0xFF
		Xorwf tev_l,0
		Btfsc STATUS,2
		Incf tev_h,1
		Incf tev_l,1
		Goto Te_Vis
Te_dv	Movlw 0xFE
		Xorwf tev_h,0
		Btfss STATUS,2
		Goto Te_d
		Movlw 0x99		;Controllo limite inferiore
		Xorwf tev_l,0
		Btfss STATUS,2
		Goto Te_d
		;Clrf tev_h		;Impostazione ciclica
		;Clrf tev_l		;Impostazione ciclica
		Goto Te_Vis
Te_d	Movlw 0x00
		Xorwf tev_l,0
		Btfsc STATUS,2
		Decf tev_h,1
		Decf tev_l,1
		Goto Te_Vis
TeVsKp	Movf tev_h,0	;Testing
		Movwf tmp5		;Testing
		Movf tev_l,0	;Testing
		Movwf tmp4		;Testing
		Btfss tev_h,7
		Goto TeCtrl
		Comf tev_l,1		;Complemento a 2 parte intera
		Movlw 0x01
		Addwf tev_l,1
		Comf tev_h,1
		Movlw 0x01
		Btfsc STATUS,0
		Addwf tev_h,1
TeCtrl	Clrf Ind
		Movlw 0x00
		Call ERead
		Movwf K
		Btfsc tev_h,0	;Controllo centinaia angolo desiderato
		Goto Te100
		Movlw 0x01		;teta=1
		Xorwf tev_l,0
		Btfss STATUS,2
		Goto Te10
		Movlw 0x96	;Kp=150
		Goto Te1xx		
Te10	Movlw 0x0A		;(Te10)
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<10
		Goto Te15		;teta>9	
		Movlw 0xBE		;Kp<190
		Subwf K,0
		Btfsc STATUS,0
		Goto Set120
		Movlw 0x46		;Kp>70
		Subwf K,0
		Btfsc STATUS,0
		Return
Set120	Movlw 0x50	;Kp=80
		Goto Te1xx
Te15	Movlw 0x0F
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<15
		Goto Te25		;teta>14
		Movlw 0x78		;Kp<120
		Subwf K,0
		Btfsc STATUS,0
		Goto Set50
		Movlw 0x0F		;Kp>15
		Subwf K,0
		Btfsc STATUS,0
		Return
Set50	Movlw 0x19	;Kp=25
		Goto Te1xx
Te25	Movlw 0x19
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<25
		Goto Te50		;teta>24
		Movlw 0x46		;Kp<70
		Subwf K,0
		Btfsc STATUS,0
		Goto Set40
		Movlw 0x0A		;Kp>10
		Subwf K,0
		Btfsc STATUS,0
		Return
Set40	Movlw 0x14	;Kp=20
		Goto Te1xx
Te50	Movlw 0x32
		Subwf tev_l,0
		Btfsc STATUS,0	;teta<50
		Goto Te100		;teta>49
		Movlw 0x23		;Kp<35
		Subwf K,0
		Btfsc STATUS,0
		Goto Set20
		Movlw 0x06		;Kp>6
		Subwf K,0
		Btfsc STATUS,0
		Return
Set20	Movlw 0x0A	;Kp=10
		Goto Te1xx
;CU100	Movlw 0x64
;		Subwf tev_l,0
;		Btfsc STATUS,2	;teta<100
;		Goto CU150		;teta>99
Te100	Movlw 0x05	;Kp=5
Te1xx	Call EWrite
		Return
;*******Procedura cancellazione display*******
DspClr	Bcf Port_A,2
		Movlw 0x10	;Display Clear
		Call WrDisp
		Bsf Port_A,2
		Return
Del2	Movwf D2
DelR2	Movlw 0xFF	;Routine doppio ritardo
		Call Del1
		Decfsz D2,1
		Goto DelR2
		Return
Del1	Movwf D1	;Routine singolo ritardo
DelR1	Movlw 0xFF
		Call Del0
		Decfsz D1,1
		Goto DelR1
		Return
Del0	Movwf D0	;Routine ritardo
DelR0	Decfsz D0,1
		Goto DelR0
		Return
;*******Cancellazione linea sul display*******
ClrPLn	Call PRiga
		Call ClrLn
		Goto PRiga
ClrSLn	Call SRiga
		Call ClrLn
		Goto SRiga
ClrLn	Movlw 0x10
		Movwf Cnt
Clr		Movlw 0x02	;(20h Space)
		Call WrDisp
		Decfsz Cnt,1
		Goto Clr
		Return
;*******Procedura scrittura caratteri/istruzioni su display*******
WrDisp	Movwf Write
		Call SetPC
		Swapf Write,1
SetPC	Bcf Port_B,4
		Btfsc Write,0
		Bsf Port_B,4
		Bcf Port_B,5
		Btfsc Write,1
		Bsf Port_B,5
		Bcf Port_B,6
		Btfsc Write,2
		Bsf Port_B,6
		Bcf Port_B,7
		Btfsc Write,3
		Bsf Port_B,7
Enable	Bsf Port_A,4
		Movlw 0x30	;Test (0x08)
		Call Del1	;Test
		Bcf Port_A,4
		Return
;*******Procedure settaggio posizioni display*******
SetPos	Swapf Pos,0
		Call Riga
		Return
Riga	Bcf Port_A,2
		Call WrDisp
		Bsf Port_A,2
		Return
PRiga	Movlw 0x80	;1a riga
		Movwf Pos
		Swapf Pos,0
		Call Riga
		Return
SRiga	Movlw 0xC0	;2a riga
		Movwf Pos
		Swapf Pos,0
		Call Riga
		Return
;*******Cancellazione EEPROM*******
EClear	Movlw 0x9F
		Movwf Ind
EClr	Movlw 0x02
		Call EWrite
		Decfsz Ind,1
		Goto EClr
		Movlw 0x02
		Call EWrite
		Return
;*******Lettura EEPROM*******
ERead	Bsf STATUS,6
		Movwf EEADR
		Bsf STATUS,5
		Bcf EECON1,7
		Bsf EECON1,0
		Bcf STATUS,5
		Movf EEDATA,0
		Bcf STATUS,6
		Return
;*******Scrittura EEPROM*******
EWrite	;Bcf Intcon,7	;Test
		Bsf STATUS,6
		Movwf EEDATA
		Bcf STATUS,6
		Movf Ind,0
		Bsf STATUS,6
		Movwf EEADR
		Bsf STATUS,5
		Bcf EECON1,7
		Bsf EECON1,2
		Movlw 0x55
		Movwf EECON2
		Movlw 0xAA
		Movwf EECON2
		Bsf EECON1,1
		Bcf EECON1,2
		Btfsc EECON1,1
		Goto $-1
		Bcf STATUS,5
		Bcf STATUS,6
		;Bsf Intcon,7	;Test
		Return
		Org 1800h
;*******Messaggio di avvio*******
Disp	Bsf Port_A,2
		Bcf PCLATH,3
		Movlw 0xC4	;(4Ch L)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
		Movlw 0x26	;(62h b)
		Call WrDisp
		Movlw 0xE2	;(2Eh .)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
		Movlw 0x57	;(75h u)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0xD6	;(6Dh m)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x96	;(69h i)
		Call WrDisp
		Movlw 0x36	;(63h c)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
MMEVs	Bcf PCLATH,3
		Movlw 0x1C
		Call Riga
		Movlw 0x22	;(22h ")
		Call WrDisp
		Movlw 0xC4	;(4Ch L)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Movlw 0x35	;(53h S)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
		Movlw 0x07	;(70h p)
		Call WrDisp
		Movlw 0x96	;(69h i)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0xE6	;(6Eh n)
		Call WrDisp
		Movlw 0xA7	;(7Ah z)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
		Movlw 0x22	;(22h ")
		Call WrDisp
		Return
MME_Vis	Bcf PCLATH,3	;(TFT)
		Movlw 0xD4	;(4Dh M)
		Call WrDisp
		Movlw 0xE2	;(2Eh .)
		Call WrDisp
		Movlw 0xD4	;(4Dh M)
		Call WrDisp
		Movlw 0xE2	;(2Eh .)
		Call WrDisp
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Movlw 0x54	;(45h E)
		Call WrDisp
		Movlw 0xC6	;(6Ch l)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x36	;(63h c)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x27	;(72h r)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0xE6	;(6Eh n)
		Call WrDisp
		Movlw 0x96	;(69h i)
		Call WrDisp
		Movlw 0x36	;(63h c)
		Call WrDisp
		Movlw 0x37	;(73h s)
		Call WrDisp
		Return
Site	Bcf PCLATH,3
		Movlw 0xE7	;(7Eh ->)
		Call WrDisp
		Movlw 0x77	;(77h w)
		Call WrDisp
		Movlw 0x77	;(77h w)
		Call WrDisp
		Movlw 0x77	;(77h w)
		Call WrDisp
		Movlw 0xE2	;(2Eh .)
		Call WrDisp
		Movlw 0xD6	;(6Dh m)
		Call WrDisp
		Movlw 0xD6	;(6Dh m)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x66	;(66h f)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0xE2	;(2Eh .)
		Call WrDisp
		Movlw 0x96	;(69h i)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Return
PID_Vs	Bcf PCLATH,3
		Movlw 0x94	;(49h I)
		Call WrDisp
		Movlw 0xD6	;(6Dh m)
		Call WrDisp
		Movlw 0x07	;(70h p)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0x37	;(73h s)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
		Bsf PCLATH,3
		Btfsc MME,0
		Goto PID_1st
		Bcf PCLATH,3
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Bsf PCLATH,3
		Btfsc Aux,0
		Goto kp_Vs
		Btfsc Aux,1
		Goto ki_Vs
		Btfsc Aux,2
		Goto kd_Vs
PID_1st	Bcf PCLATH,3
		Bcf Ctrl,0
		Movlw 0xA7	;(7Ah z)
		Call WrDisp
		Movlw 0x96	;(69h i)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0xE6	;(6Eh n)
		Call WrDisp				
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Movlw 0x05	;(50h P)
		Call WrDisp
		Movlw 0x94	;(49h I)
		Call WrDisp
		Movlw 0x44	;(44h D)
		Call WrDisp
		Return
kp_Vs	Bcf PCLATH,3
		Movlw 0xB6	;(6Bh k)
		Call WrDisp
		Movlw 0x07	;(70h p)
		Call WrDisp
		Movlw 0xA3	;(3Ah :)
		Call WrDisp
		Return
kd_Vs	Bcf PCLATH,3
		Movlw 0xB6	;(6Bh k)
		Call WrDisp
		Movlw 0x46	;(64h d)
		Call WrDisp
		Movlw 0xA3	;(3Ah :)
		Call WrDisp
		Return
ki_Vs	Bcf PCLATH,3
		Movlw 0xB6	;(6Bh k)
		Call WrDisp
		Movlw 0x96	;(69h i)
		Call WrDisp
		Movlw 0xA3	;(3Ah :)
		Call WrDisp
		Return
Teta_Vs	Bcf PCLATH,3
		Movlw 0x14	;(41h A)
		Call WrDisp
		Movlw 0xE6	;(6Eh n)
		Call WrDisp
		Movlw 0x76	;(67h g)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0xC6	;(6Ch l)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Movlw 0x46	;(64h d)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x37	;(73h s)
		Call WrDisp
		Movlw 0x96	;(69h i)
		Call WrDisp	
		Movlw 0x46	;(64h d)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x27	;(72h r)
		Call WrDisp
		Movlw 0xE2	;(2Eh .)
		Call WrDisp
		Movlw 0xA3	;(3Ah :)
		Call WrDisp
		Return
Enc_Rd	Bcf PCLATH,3
		Movlw 0xC4	;(4Ch L)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x57	;(75h u)
		Call WrDisp
		Movlw 0x27	;(72h r)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
		Bsf PCLATH,3
		Goto Enc_Sk
Enc_Vs	Bcf PCLATH,3
		Movlw 0x94	;(49h I)
		Call WrDisp
		Movlw 0xD6	;(6Dh m)
		Call WrDisp
		Movlw 0x07	;(70h p)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0x37	;(73h s)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x16	;(61h a)
		Call WrDisp
Enc_Sk	Bcf PCLATH,3
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0xE6	;(6Eh n)
		Call WrDisp
		Movlw 0x36	;(63h c)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0x46	;(64h d)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x27	;(72h r)
		Call WrDisp
		Movlw 0xA3	;(3Ah :)
		Call WrDisp
		Return
Res_Vs	Bcf PCLATH,3
		Movlw 0x35	;(53h S)
		Call WrDisp
		Movlw 0x9F	;(F9h y)
		Call WrDisp
		Movlw 0x37	;(73h s)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0xD6	;(6Dh m)
		Call WrDisp
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Movlw 0x27	;(72h r)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x37	;(73h s)
		Call WrDisp		
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0x12	;(21h !)
		Call WrDisp
		Movlw 0xA3	;(3Ah :)
		Call WrDisp
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Movlw 0x02	;(20h Space)
		Call WrDisp
		Return
Exe		Bcf PCLATH,3
		Movlw 0x54	;(45h E)
		Call WrDisp
		Movlw 0x37	;(73h s)
		Call WrDisp
		Movlw 0x56	;(65h e)
		Call WrDisp
		Movlw 0x76	;(67h g)
		Call WrDisp
		Movlw 0x57	;(75h u)
		Call WrDisp
		Movlw 0x96	;(69h i)
		Call WrDisp
		Movlw 0x47	;(74h t)
		Call WrDisp
		Movlw 0xF6	;(6Fh o)
		Call WrDisp
		Movlw 0x12	;(21h !)
		Call WrDisp
		Return
		End
		Org 1FFFh
		Goto Ini