PROCESSOR       16F84
        RADIX           DEC
        INCLUDE         "P16F84.INC"
        ERRORLEVEL      -302

        ;Setup of PIC configuration flags

        ;XT oscillator
        ;Disable watch dog timer
        ;Enable power up timer
        ;Disable code protect

        __CONFIG        3FF1H

LED     EQU     0

        ORG     0CH

Count   RES     2
Count1	RES	1
Count2  RES     1
Servo	RES	1	;tmp per SetDir
Servo01	RES	1	;base braccio
Servo02	RES	1	;braccio
Servo03	RES	1	;avanbraccio
Servo04	RES	1	;avanbraccio
Sit	RES	1	;byte di movimenti, tmp per PORTB1
Verif1	RES	1	;byte di verifica: quantitą di "1" caricata
Verif2	RES	1	;byte di verifica: quantitą di "1" calcolata
ServT1a	RES	1	;tmp per comparazione Verif 1-2
ServT1b	RES	1	;tmp per comparazione Verif 1-2

        ;Reset Vector
        ;Start point at CPU reset

        ORG     00H

        bsf     STATUS,RP0
        movlw   00000000B
        movwf   TRISA 
        movlw   11001000B
        movwf   TRISB 
        bcf     STATUS,RP0


	bcf	PORTA,0
	bcf	PORTA,1	;led errore
	bcf	PORTA,2	;curva a sinistra
	bcf	PORTB,0
	bcf	PORTB,1	;vai avanti
	bcf	PORTB,2	;vai indietro
	;bcf	PORTB,3 ;ricezione RX
	bcf	PORTB,4 ;trasmissione TX
	bcf	PORTB,5	;output SDO (SincroDataOutput)
	;bcf	PORTB,6	;input SDI (SincroDataInput)
	;bcf	PORTB,7	;input-output DIO (DataInputOutput) Default Input

	;startup
	bsf	PORTA,1	;led errore
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	call	Delay1
	bcf	PORTA,1	;led errore

MainLoop

        btfsc   PORTB,6
	call	SetDir	;dati in arrivo

	goto	MainLoop

SetDir

	;cancello verif1 e 2
	clrf	Verif1
	clrf	Verif2
	
	;Settaggio Servo01 bit 0
	btfsc	PORTB,7
	bsf	Servo01,0
	btfsc	PORTB,7
	incf	Verif2,F   ;incrementa solo in presenza di un "1"
	btfss	PORTB,7
	bcf	Servo01,0

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo01 bit 1
	btfsc	PORTB,7
	bsf	Servo01,1
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo01,1

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo01 bit 2
	btfsc	PORTB,7
	bsf	Servo01,2
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo01,2

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo01 bit 3
	btfsc	PORTB,7
	bsf	Servo01,3
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo01,3

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo01 bit 4
	btfsc	PORTB,7
	bsf	Servo01,4
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo01,4

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo01 bit 5
	btfsc	PORTB,7
	bsf	Servo01,5
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo01,5

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo01 bit 6
	btfsc	PORTB,7
	bsf	Servo01,6
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo01,6

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo01 bit 7
	btfsc	PORTB,7
	bsf	Servo01,7
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo01,7

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo02 bit 0
	btfsc	PORTB,7
	bsf	Servo02,0
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo02,0

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo02 bit 1
	btfsc	PORTB,7
	bsf	Servo02,1
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo02,1

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo02 bit 2
	btfsc	PORTB,7
	bsf	Servo02,2
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo02,2

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo02 bit 3
	btfsc	PORTB,7
	bsf	Servo02,3
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo02,3

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo02 bit 4
	btfsc	PORTB,7
	bsf	Servo02,4
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo02,4

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo02 bit 5
	btfsc	PORTB,7
	bsf	Servo02,5
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo02,5

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo02 bit 6
	btfsc	PORTB,7
	bsf	Servo02,6
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo02,6

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo02 bit 7
	btfsc	PORTB,7
	bsf	Servo02,7
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo02,7

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo03 bit 0
	btfsc	PORTB,7
	bsf	Servo03,0
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo03,0

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo03 bit 1
	btfsc	PORTB,7
	bsf	Servo03,1
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo03,1

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo03 bit 2
	btfsc	PORTB,7
	bsf	Servo03,2
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo03,2

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo03 bit 3
	btfsc	PORTB,7
	bsf	Servo03,3
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo03,3

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo03 bit 4
	btfsc	PORTB,7
	bsf	Servo03,4
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo03,4

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo03 bit 5
	btfsc	PORTB,7
	bsf	Servo03,5
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo03,5

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo03 bit 6
	btfsc	PORTB,7
	bsf	Servo03,6
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo03,6

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo03 bit 7
	btfsc	PORTB,7
	bsf	Servo03,7
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo03,7

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo04 - Pinza bit 0=si muove
	btfsc	PORTB,7
	bsf	Servo04,0
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo04,0

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo04 - Pinza bit 1=chiude o apre
	btfsc	PORTB,7
	bsf	Servo04,1
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo04,1

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo04 - Avanti bit 2=vai avanti
	btfsc	PORTB,7
	bsf	Servo04,2
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo04,2

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo04 - Indietro bit 3=vai indietro
	btfsc	PORTB,7
	bsf	Servo04,3
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo04,3

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo04 - Avanti bit 4=vai a destra
	btfsc	PORTB,7
	bsf	Servo04,4
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo04,4

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo04 - Indietro bit 5=vai a sinistra
	btfsc	PORTB,7
	bsf	Servo04,5
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo04,5

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo04 - Centra bit 6=curva e centra le ruote
	btfsc	PORTB,7
	bsf	Servo04,6
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo04,6

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Servo04 - Fast Servo bit 7=posiziona i servo alla massima velocitą
	btfsc	PORTB,7
	bsf	Servo04,7
	btfsc	PORTB,7
	incf	Verif2,F
	btfss	PORTB,7
	bcf	Servo04,7

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Verif - bit 0 - N° di "1" della parola
	btfsc	PORTB,7
	bsf	Verif1,0
	btfss	PORTB,7
	bcf	Verif1,0

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Verif - bit 1
	btfsc	PORTB,7
	bsf	Verif1,1
	btfss	PORTB,7
	bcf	Verif1,1

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Verif - bit 2
	btfsc	PORTB,7
	bsf	Verif1,2
	btfss	PORTB,7
	bcf	Verif1,2

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Verif - bit 3
	btfsc	PORTB,7
	bsf	Verif1,3
	btfss	PORTB,7
	bcf	Verif1,3

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Verif - bit 4
	btfsc	PORTB,7
	bsf	Verif1,4
	btfss	PORTB,7
	bcf	Verif1,4

	call	WaitRB6off
	call	WaitRB6on

	;Settaggio Verif - bit 5
	btfsc	PORTB,7
	bsf	Verif1,5
	btfss	PORTB,7
	bcf	Verif1,5

	call	WaitRB6off

	;trasferisco i valori per praticitą (ctrl-c ctrl-v)
	movf	Verif1,0
	movwf	ServT1a
	movf	Verif2,0
	movwf	ServT1b

	;compara Verif1 e 2
	movf	ServT1a,0   ;ServT1a -> W (,0)
	subwf	ServT1b,1   ;ServT1b - W ->ServT1b (,1)
	
	;se le sottrazioni danno 0 -> dati ok!
	
	btfsc	ServT1b,0
	goto	Errore
	btfsc	ServT1b,1
	goto	Errore
	btfsc	ServT1b,2
	goto	Errore
	btfsc	ServT1b,3
	goto	Errore
	btfsc	ServT1b,4
	goto	Errore

	;ServT1a e ServT1b sono uguali!
	bcf	PortA,1   ;spegne l'eventuale led errore acceso

	;fischio = 35mS
	bsf	PORTB,4
	call	delay1
	call	delay1
	;call	delay1
	bcf	PORTB,4
	call	delayR
	
	;invio dati servo01
	movf	Servo01,0
	movwf	Servo
	call	LoadServo

	;invio dati servo02
	movf	Servo02,0
	movwf	Servo
	call	LoadServo

	;invio dati servo03
	movf	Servo03,0
	movwf	Servo
	call	LoadServo

	;invio dati pinza
	btfss	Servo04,0
	bcf	PORTB,4
	btfsc	Servo04,0
	bsf	PORTB,4
	call	DelayR

	btfss	Servo04,1
	bcf	PORTB,4
	btfsc	Servo04,1
	bsf	PORTB,4
	call	DelayR

	;invio dati avanti/indietro
	btfss	Servo04,2
	bcf	PORTB,4
	btfsc	Servo04,2
	bsf	PORTB,4
	call	DelayR

	btfss	Servo04,3
	bcf	PORTB,4
	btfsc	Servo04,3
	bsf	PORTB,4
	call	DelayR

	;invio dati destra/sinistra/centra
	btfss	Servo04,4
	bcf	PORTB,4
	btfsc	Servo04,4
	bsf	PORTB,4
	call	DelayR

	btfss	Servo04,5
	bcf	PORTB,4
	btfsc	Servo04,5
	bsf	PORTB,4
	call	DelayR

	btfss	Servo04,6
	bcf	PORTB,4
	btfsc	Servo04,6
	bsf	PORTB,4
	call	DelayR

	;invio dato Fast servo
	btfss	Servo04,7
	bcf	PORTB,4
	btfsc	Servo04,7
	bsf	PORTB,4
	call	DelayR

	;invio dati di verifica (N° di "1") bit 0
	btfss	Verif1,0
	bcf	PORTB,4
	btfsc	Verif1,0
	bsf	PORTB,4
	call	DelayR

	;verifica bit 1
	btfss	Verif1,1
	bcf	PORTB,4
	btfsc	Verif1,1
	bsf	PORTB,4
	call	DelayR

	;verifica bit 2
	btfss	Verif1,2
	bcf	PORTB,4
	btfsc	Verif1,2
	bsf	PORTB,4
	call	DelayR

	;verifica bit 3
	btfss	Verif1,3
	bcf	PORTB,4
	btfsc	Verif1,3
	bsf	PORTB,4
	call	DelayR

	;verifica bit 4
	btfss	Verif1,4
	bcf	PORTB,4
	btfsc	Verif1,4
	bsf	PORTB,4
	call	DelayR

	;verifica bit 5
	btfss	Verif1,5
	bcf	PORTB,4
	btfsc	Verif1,5
	bsf	PORTB,4
	call	DelayR


	bcf	PORTB,4

	goto	Uscita

Errore
	bsf	PortA,1

Uscita

	return

;LoadServo
LoadServo
	btfss	Servo,0
	bcf	PORTB,4
	btfsc	Servo,0
	bsf	PORTB,4
	call	DelayR

	btfss	Servo,1
	bcf	PORTB,4
	btfsc	Servo,1
	bsf	PORTB,4
	call	DelayR

	btfss	Servo,2
	bcf	PORTB,4
	btfsc	Servo,2
	bsf	PORTB,4
	call	DelayR

	btfss	Servo,3
	bcf	PORTB,4
	btfsc	Servo,3
	bsf	PORTB,4
	call	DelayR

	btfss	Servo,4
	bcf	PORTB,4
	btfsc	Servo,4
	bsf	PORTB,4
	call	DelayR

	btfss	Servo,5
	bcf	PORTB,4
	btfsc	Servo,5
	bsf	PORTB,4
	call	DelayR

	btfss	Servo,6
	bcf	PORTB,4
	btfsc	Servo,6
	bsf	PORTB,4
	call	DelayR

	btfss	Servo,7
	bcf	PORTB,4
	btfsc	Servo,7
	bsf	PORTB,4
	call	DelayR

	
	return

;LoadVerif
LoadVerif
	bcf	PORTB,4
	call	DelayR

	bsf	PORTB,4
	call	DelayR

	bcf	PORTB,4
	call	DelayR

	
	return



WaitRB6off
ciclo1
	btfsc	PORTB,6
	goto	ciclo1
	
	return


WaitRB6on
ciclo2
	btfss	PORTB,6
	goto	ciclo2
	
	return




        ;Software delay

Delay
        clrf    Count
        clrf    Count+1

DelayLoop

        decfsz  Count,1
        goto    DelayLoop

        decfsz  Count+1,1
        goto    DelayLoop


        return

;Attende 30ms e controlla segnale
Delay1
	;Count1=0 ecc...
	movlw	200  ;era 100
	movwf	Count1

	movlw	20
	movwf	Count2

DelayLoop1

        decfsz  Count1,1
        goto    DelayLoop1

	movlw	200 ;era 100
	movwf	Count1

	;se c'e' segnale chiama SetAng
	;btfsc	PORTA,0
	;call	SetAng

        decfsz  Count2,1
        goto    DelayLoop1

        return


;Ritardo next bit
DelayR
	movlw	100
	movwf	Count1

DelayLoopR

        decfsz  Count1,1
        goto    DelayLoopR

	return



        END