;*************************
; COMANDA 2 SERVO_COMANDI XT 4 MHZ
; rb3 ed rb4 out motore ; 
; rb1 + on avvia ricerca velocità minima
; rb1-2 ricerca velocità motore ds
; rb6 + on avvia ricerca velocità massima
; rb5-6 ricerca velocità motore sn
; rb7 memorizza velocita massima o minima
; rb2 + on avvia regolazione sensori
; ra2 ed ra3 led mot.ds e led mot.sn
; ra0 ed ra1 ingresso adc sensori
; con led al centro, Vin 1,5 V circa

; legge da adresl tutti gli 8 bit
; centratura  posizione perfetta
; centratura gestita nell'interrupt
;    O T T I M O
; IT9DPX  #135 - FRANCESCO -  12 DIC 2011
;*************************

   PROCESSOR       16F88
   RADIX           DEC
   INCLUDE         "P16F88.INC" 

   ERRORLEVEL      -302
   ERRORLEVEL      -305

   
    __CONFIG  H'2007', H'3F41'  
    __CONFIG  H'2008', H'3FFC'

   ORG 2100H
   ;scrive su eeprom dati
   DE  0x4D,0x72,0x41,0x61          
    

      ORG   0070H
;mem comune ai quattro banchi
RSE1    RES   1
RSE2    RES   1
VEL1    RES   1
VEL2    RES   1
FER1    RES   1
FER2    RES   1

  
      ORG   0020H

PASSO  RES   1
FLAG   RES   1
RITH   RES   1
RITL   RES   1
SENS1  RES   1
SENS2  RES   1
RITA   RES   1
RITB   RES   1
STEP   RES   1

;flag,0 = mot_ds/mot_sn
;flag,1 = esclude adc
;flag,2 = velocità max
;flag,3 = velocità min
;flag,4 = regolaz. sensori

#define  U_SERV_1     PORTB,3 ;out_SERVO_1
#define  U_SERV_2     PORTB,4 ;out_SERVO_2


        ORG     00H
        GOTO    VIA

        ORG     04
;OGNI 11.037 uS si ha l'interrupt di tmr0
INT
        MOVLW   .210 ; 11mS circa
        MOVWF   TMR0
        BCF     INTCON,2 ;tmr0 IF
        BSF     INTCON,7 ;global IE

   BTFSS   FLAG,0
   GOTO    SERVO_1
  ; GOTO    SERVO_2
;----------------------
SERVO_2
   BCF     FLAG,0
     BSF     U_SERV_2
   CALL    RIT_FIS
;inizio ritardo variabile
   MOVFW   RSE2
   CALL    RIT_VAR
     BCF     U_SERV_2
   GOTO    FIN_RET
;------------------------
SERVO_1
   BSF     FLAG,0
     BSF     U_SERV_1
   CALL    RIT_FIS
;inizio ritardo variabile  
   MOVFW   RSE1
   CALL    RIT_VAR
     BCF     U_SERV_1
   GOTO    FIN_RET
;------------------------
RIT_VAR
   MOVWF   RITL
R_SER2
   NOP
   DECFSZ  RITL
   GOTO    R_SER2
;fine ritardo variabile  
   RETURN
;-----------------------
RIT_FIS
   MOVLW   0X4C
   MOVWF   RITL
   MOVLW   .2
   MOVWF   RITH
; ritardo fisso 1 mS
FINE_2
   DECFSZ  RITL
   GOTO    FINE_2
   DECFSZ  RITH
   GOTO    FINE_2
   RETURN
;--- fine ritardo fisso ---

FIN_RET

;----inizio lettura dei due sensori----

   BTFSC  FLAG,1 ;esclude letture e setta velocità
   RETFIE

   CALL    BANK1
   MOVLW   00000001B
   MOVWF    ANSEL
 ;gp0 =analog.input   CALL    BANK0 
   MOVLW   00000001B
   MOVWF    ADCON0
 ;adc on - su ra0
   CALL    AVVIA_ADC
   MOVWF   SENS1

   CALL    BANK1
   MOVLW   00000010B
   MOVWF    ANSEL
 ;gp1 =analog.input   CALL    BANK0  
   MOVLW   00001001B
   MOVWF    ADCON0
 ;adc on - su ra1
   CALL    AVVIA_ADC
   MOVWF   SENS2
  

;------confronto letture-----
   MOVFW    SENS2
   SUBWF   SENS1,W  ;confronta letture
   BTFSC   STATUS,Z ;salta se sono diverse
   GOTO    UGUALI ;esegui se sono uguali
   BTFSS   STATUS,C
   GOTO    MINORE ;esegui se SENS1 è minore
   GOTO    MAGGIORE ;esegui se SENS1 è maggiore

UGUALI
   BCF    PORTA,2
   BCF    PORTA,3
     BTFSC   FLAG,4
     RETFIE
   GOTO   UGUALI_B
MINORE
     MOVFW   SENS1
     SUBWF   SENS2,W
     MOVWF   STEP
   BSF    PORTA,2
   BCF    PORTA,3
     BTFSC   FLAG,4
     RETFIE
   GOTO   SINISTRA_B
MAGGIORE
     MOVWF   STEP
   BCF    PORTA,2
   BSF    PORTA,3
     BTFSC   FLAG,4
     RETFIE
   GOTO   DESTRA_B
;///////////////////////////////////

UGUALI_B 
;...ra0 ed ra1  off ...letture uguali..
   MOVFW   VEL1 ;mot_s RSE1 ;mot_s
   MOVWF   RSE1 ;mot_s
   MOVFW   VEL2 ;mot_d RSE2 ;mot_d
   MOVWF   RSE2 ;mot_d
   RETFIE
;------fine confronto letture-----
DESTRA_B
   MOVFW   STEP
   SUBWF   VEL2,W
   MOVWF   RSE2
 BCF     STATUS,C
 RRF     STEP,W
 ADDWF   VEL1,W ;velocità in curva aumentata
 MOVWF   RSE1
  ;   MOVFW   VEL1 ;velocità in curva costante
  ;   MOVWF   RSE1
  RETFIE
;--------------------
SINISTRA_B
   MOVFW   STEP
   SUBWF   VEL1,W
   MOVWF   RSE1
 BCF     STATUS,C
 RRF     STEP,W
 ADDWF   VEL2,W ;velocità in curva aumentata
 MOVWF   RSE2
  ;   MOVFW   VEL2 ;velocità in curva costante
  ;   MOVWF   RSE2
  RETFIE

;----------------------
AVVIA_ADC
   BSF    ADCON0,2 ;avvia conv.
CE_DATO
   BTFSC  ADCON0,2
   GOTO   CE_DATO  ;conv. in corso
   BCF   STATUS,C
   CALL   BANK1
   MOVFW    ADRESL
   CALL   BANK0
   RETURN
;------------------------

VIA      
     CALL  BANK1
   MOVLW  0007
   MOVWF  CMCON
 ;spegne compar. porte I/O on
   MOVLW   00000000B
   MOVWF    ANSEL
 ; ingressi digitali
   MOVLW   10000000B
   MOVWF    ADCON1
 ; 1 right 0 Left justified. 
   MOVLW   7 ;pull-up
   MOVWF   OPTION_REG
   MOVLW  B'11100111'
   MOVWF   TRISB
   MOVLW  B'00010011'
   MOVWF   TRISA 
      CALL  BANK0
   CLRF    PORTB
   CLRF    PORTA
      
;------------------
    CLRF  FLAG
    CALL  LEGGO

    ;   CALL  MEM_VEL ;DESTRA  
;|||||||||||||||||||||||||||||

   CALL    CONTINUA_B
   CALL    CONTINUA_B


   BSF     INTCON,5 ;tmr0 IE
   BSF     INTCON,7 ;global IE
 ;accende interrupt
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;setta flag_1 = regolazione sensori
   btfsc  PORTB,2
   GOTO   PB_SEI
   BSF    FLAG,4
     MOVFW   FER1
     MOVWF   RSE1
     MOVFW   FER2
     MOVWF   RSE2
CTRL_D
   CALL   CONTINUA_B
   btfss  PORTB,2
   GOTO   CTRL_D

;setta flag_1 = regolazioni velocità massima
PB_SEI
   btfsc  PORTB,6
   GOTO   PB_UNO
   BSF    FLAG,1
   BSF    FLAG,2
     MOVFW   VEL1
     MOVWF   RSE1
     MOVFW   VEL2
     MOVWF   RSE2
CTRL_C
   CALL   CONTINUA_B
   btfss  PORTB,6
   GOTO   CTRL_C


;setta flag_1 = regolazioni velocità minima
PB_UNO
   btfsc  PORTB,1
   GOTO   TROVA
   BSF    FLAG,1
   BSF    FLAG,3
     MOVFW   FER1
     MOVWF   RSE1
     MOVFW   FER2
     MOVWF   RSE2
CTRL_B
   CALL   CONTINUA_B
   btfss  PORTB,1
   GOTO   CTRL_B


;*****************************     
TROVA
   BTFSS  FLAG,1 ;esclude letture e setta velocità
   GOTO   TROVA  ;CONTINUA
      BCF    PORTA,3
      CALL   CONTINUA_B

   btfsc  PORTB,7
   goto   SALTA

   btfsc  FLAG,2
   CALL   MEM_VEL
   btfsc  FLAG,3
   CALL   MEM_M_FER

SALTA
;controllo velocità serv1
   btfss   PORTB,2
   incf    RSE1
   btfss   PORTB,1
   decf    RSE1
;fine controllo velocità serv1

;controllo velocità serv2
   btfss   PORTB,5
   incf    RSE2
   btfss   PORTB,6
   decf    RSE2
;fine controllo velocità serv2
   GOTO   TROVA

CONTINUA_B
     MOVLW   .4
     XORWF   PORTA,F
   MOVLW   .255
   MOVWF   RITA
   MOVLW   .200
   MOVWF   RITB
ANCORA_B
   DECFSZ   RITA
   GOTO    ANCORA_B
   DECFSZ   RITB
   GOTO    ANCORA_B
      RETURN

;.......................
MEM_VEL
   btfss  PORTB,7
   GOTO   MEM_VEL
   MOVFW  RSE1
   MOVWF  VEL1  
   MOVFW  RSE2
   MOVWF  VEL2 
;-----------------------    
SALVA
   BCF     INTCON,GIE
     CALL    BANK2
     CLRF    EEADR
     DECF    EEADR
   MOVFW   VEL1
   CALL  SCRIVO
   MOVFW   VEL2
   CALL   SCRIVO
     MOVFW   FER1
     CALL  SCRIVO
     MOVFW   FER2
     CALL   SCRIVO
   CALL    BANK0
   BSF     INTCON,GIE
   RETURN
;--------------
SCRIVO
   MOVWF   EEDATA
   INCF    EEADR
      CALL    BANK3
   BCF     EECON1,EEPGD
   BSF     EECON1,WREN
   MOVLW   0x55
   MOVWF   EECON2
   MOVLW   0xAA
   MOVWF   EECON2
   BSF     EECON1,WR
LPW2
   BTFSC   EECON1,WR
   GOTO    LPW2
   BCF     EECON1,WREN
     CALL    BANK2
   RETURN
;---------------------------

MEM_M_FER
   btfss  PORTB,0
   GOTO   MEM_M_FER
   MOVFW  RSE1
   MOVWF  FER1  
   MOVFW  RSE2
   MOVWF  FER2
   GOTO   SALVA
;-----------------------    
LEGGO
   CALL    BANK2
   CLRF    EEADR
   DECF    EEADR
     CALL    LEGGI
   MOVWF   RSE1
   MOVWF   VEL1
     CALL    LEGGI
   MOVWF   RSE2
   MOVWF   VEL2
     CALL    LEGGI
   MOVWF   FER1
     CALL    LEGGI
   MOVWF   FER2
     CALL    BANK0
   RETURN

LEGGI
   INCF    EEADR
      CALL    BANK3
   BCF     EECON1,EEPGD
   BSF     EECON1,RD
      CALL    BANK2
   MOVFW   EEDATA
   RETURN

;----------------------

BANK0
   BCF     STATUS,RP1
   BCF     STATUS,RP0
   RETURN
BANK1
   BCF     STATUS,RP1
   BSF     STATUS,RP0
   RETURN
BANK2
   BSF     STATUS,RP1
   BCF     STATUS,RP0
   RETURN
BANK3
   BSF     STATUS,RP1
   BSF     STATUS,RP0
   RETURN
        END










