PROCESSOR       16F84
        RADIX           DEC
        INCLUDE         "P16F84a.INC"
        INCLUDE         "pwrasm.inc"
        ERRORLEVEL      -302

        ;Setup of PIC configuration flags

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

        __CONFIG        3FF5H


        ORG     0CH

;Contatori
Count	RES	1
Count1  RES     1
Count2  RES     1
Count4  RES     1

;Servocomandi
Servo	RES	1	;tmp per SetAng
ServT1a	RES	1	;tmp per SetAng
ServT2a	RES	1	;tmp per SetAng
ServT3a	RES	1	;tmp per SetAng
ServT4a	RES	1	;tmp per SetAng

ServT1b	RES	1	;tmp per SetAng
ServT2b	RES	1	;tmp per SetAng
ServT3b	RES	1	;tmp per SetAng
ServT4b	RES	1	;tmp per SetAng

ServAR1	RES	1	;valore che tende a servo01 (per rallentare movimento)
ServAR2	RES	1	;= x servo 2
ServAR3	RES	1	;= x servo 3

Servo01	RES	1	;base braccio
Servo02	RES	1	;braccio
Servo03	RES	1	;avanbraccio
Servo04	RES	1	;pinza

Verif1	RES	1	;byte verifica calcolato (Nฐ di "1" della parola)
Verif2	RES	1	;byte verifica ricevuto

Ok	RES	1	;byte di verifica


Autostp	RES	1	;contatore di cicli per l'auto-stop dei motori

        ;Reset Vector
        ;Start point at CPU reset

        ORG     00H

        bsf     STATUS,RP0

        movlw   11110001B
        movwf   TRISA 

        movlw   11000000B
        movwf   TRISB 

        bcf     STATUS,RP0

        ;Assign the PRESCALER to Watch dog timer

        bsf     OPTION_REG & 0x7F,PSA

        ;Set the PRESCALER to 1:128

        bsf     OPTION_REG & 0x7F,PS0
        bsf     OPTION_REG & 0x7F,PS1
        bsf     OPTION_REG & 0x7F,PS2

        bcf     STATUS,RP0      ;Swap to data bank 0

	;angoli di default
	movlw	140
	movwf	Servo01
	movwf	ServAR1
	movlw	220
	movwf	Servo02
	movwf	ServAR2
	movlw	200
	movwf	Servo03
	movwf	ServAR3
	
	;pinza ferma (0,1), motori fermi (2,3)
	movlw	0
	movwf	Servo04

	;Auto-stop motori, quando e se arriva a 0 spegne i motori
	movlw	3
	movwf	Autostp

	bcf	PORTB,0 ;servo base
	bcf	PORTB,1 ;servo avanbraccio
	bcf	PORTB,2 ;servo braccio
	bcf	PORTB,3 ;pinza
	bcf	PORTB,4 ;pinza
	bcf	PORTB,5 ;DIR, comando di scambio alimentazione motori
	bcf	PORTA,1 ;POW, comando motori
	bcf	PORTA,2 ;TURN, comando rel่ motore avanti/indietro - destra/sinistra
	bcf	PORTA,3 ;Led verde = inibizione RX = movimenti motori
	;bsf	PORTA,4 ;CENT, ricezione segnale ruote centrate

MainLoop

        clrwdt                  ;Clear watch dog timer

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

	;chiude o apre la pinza se ่ da muovere
	btfss	Servo04,0
	goto	SkPin   ;SkeepPinza
	
	;chiude pinza
	btfsc	Servo04,1
	bsf	PORTB,3
	btfss	Servo04,1
	bcf	PORTB,3
	;apre pinza
	btfsc	Servo04,1
	bcf	PORTB,4
	btfss	Servo04,1
	bsf	PORTB,4
	
SkPin

	;setta rel่ DIR e TURN in base a servo04 bit 2,3 (av/ind) e 4,5 (dx/sx)
	;stacca rel่ DIR se avanti o destra ่ impostato
	btfsc	Servo04,2
	bcf	PORTB,5
	btfsc	Servo04,4
	bcf	PORTB,5
	;attacca rel่ DIR se indietro o sinistra ่ impostato
	btfsc	Servo04,3
	bsf	PORTB,5
	btfsc	Servo04,5
	bsf	PORTB,5

	;stacca rel่ TURN
	bcf	PORTA,2
	;attacca rel่ TURN se destra o sinistra sono impostati
	btfsc	Servo04,4
	bsf	PORTA,2
	btfsc	Servo04,5
	bsf	PORTA,2

	;se centra ่ impostato e le ruote sono centrate non azionare POW
	btfss	Servo04,6
	goto	VaiPow
	btfsc	PORTA,4  ;ingresso contatto di ruote centrate
	goto	SkPow
	
VaiPow
	;se Autostp e' arrivato a 0 non attaccare i motori e resetta avanti, indietro, dx e sx
	jpfz	Autostp,resmot	;se f=0 vai a resmot

	;attacca POW se avanti, indietro, destra o sinistra ่ impostato
	btfsc	Servo04,2
	bsf	PORTA,1
	btfsc	Servo04,3
	bsf	PORTA,1
	btfsc	Servo04,4
	bsf	PORTA,1
	btfsc	Servo04,5
	bsf	PORTA,1

	;se i motori sono accesi fai countdown Autostp e continua con SkPow
	btfsc	PORTA,1
	decf	Autostp,F
	btfsc	PORTA,1
	goto	SkPow
resmot
	bcf	Servo04,2	;resetta avanti, indietro, dx e sx
	bcf	Servo04,3
	bcf	Servo04,4
	bcf	Servo04,5

SkPow
	;incremeto/decremento servo per movimento lento
	call ARServ1

	;posiziona il primo servo
	bsf	PORTB,0
	call	Delay2
	;movf	Servo01,0
	movf	ServAR1,0
	movwf	Count2
	call	Delay3
	bcf	PORTB,0

	;incremeto/decremento servo per movimento lento
	call ARServ2

	;secondo servo
	bsf	PORTB,1
	call	Delay2
	;movf	Servo02,0
	movf	ServAR2,0
	movwf	Count2
	call	Delay3
	bcf	PORTB,1

	;incremeto/decremento servo per movimento lento
	call ARServ3

	;terzo servo
	bsf	PORTB,2
	call	Delay2
	;movf	Servo03,0
	movf	ServAR3,0
	movwf	Count2
	call	Delay3
	bcf	PORTB,2
	
	;attesa
	call	Delay1
	call	Delay1
	;se centra NON ่ impostato fai altri delay
	btfss	Servo04,6
	call	Delay1


	;call	Delay1
	;call	Delay1
	;call	Delay1

	;azzera output pinza e POW
	bcf	PORTB,3
	bcf	PORTB,4
	bcf	PORTA,1

	;ferma pinza software (per essere impulsivo)
	;clrf	Servo04
	bcf	Servo04,0
	bcf	Servo04,1

	bcf	PORTA,3  ;spegne led verde

	goto	MainLoop


;****************** Subroutines


;Ritardo next bit
DelayR
	movlw	100
	movwf	Count

DelayLoopR

        decfsz  Count,1
        goto    DelayLoopR

	return


;Ritardo next bit + mezzo bit
DelayM
	movlw	150
	movwf	Count

DelayLoopM

        decfsz  Count,1
        goto    DelayLoopM

	return


;Attende 10ms
Delay1
	;Count1=0 ecc...
	movlw	200  ;era 100
	movwf	Count1

	movlw	20
	movwf	Count2

DelayLoop1

        decfsz  Count1,1
        goto    DelayLoop1

	movlw	100
	movwf	Count1

        decfsz  Count2,1
        goto    DelayLoop1

        return

;servo pos minima
Delay2
	movlw	50
	movwf	Count1

DelayLoop2
	nop
	nop
	decfsz	Count1,1
	goto	DelayLoop2

	return

;servo pos precisa
Delay3

DelayLoop3
	nop
	nop
	nop
	nop
	;nop
	decfsz	Count2,1
	goto	DelayLoop3

	return


;legge dati da radio RX e setta l'angolo
SetAng
;segnale=1 fischio da 35 ms + 3 bytes posizioni + 2 bit pinza apri/chiudi + 2 bit avanti/indietro
;        + 3 bit destra/sinistra/centra + 1 bit per FastServo + 6 bit di verifica (Nฐ di "1" della parola)

	;cancello verif1 e 2
	clrf	Verif1
	clrf	Verif2

	;attende lo zero di byte fischio
	call	ADw

	call	DelayM  ;attende un bit e mezzo

	;carica i servo in temp "a"
	
	call	LoadServo
	movf	Servo,0
	movwf	ServT1a

	call	LoadServo
	movf	Servo,0
	movwf	ServT2a

	call	LoadServo
	movf	Servo,0
	movwf	ServT3a

	;carica i 2 bit per la pinza
	call	LoadPinza

	;carica i 2 bit avanti/indietro
	call	LoadAvInd

	;carica i 3 bit destra/sinistra/centra
	call	LoadDxSx

	;carica 1 bit per fast servo
	call	LoadFS

	;carica i 6 bit per la verifica
	call	LoadVerif


	;comparazione dei verif 1 e 2
	movf	Verif1,0   ;Verif1 -> W (,0)
	subwf	Verif2,1   ;Verif2 - W ->Verif2 (,1)
	
	;se le sottrazioni danno 0 -> dati ok!
	
	btfsc	Verif2,0
	goto	Errore
	btfsc	Verif2,1
	goto	Errore
	btfsc	Verif2,2
	goto	Errore
	btfsc	Verif2,3
	goto	Errore
	btfsc	Verif2,4
	goto	Errore
	btfsc	Verif2,5
	goto	Errore
	btfsc	Verif2,6
	goto	Errore
	btfsc	Verif2,7
	goto	Errore

	;Verif1 e Verif2 sono uguali

	;controllo che non valgano 0
	bcf	Ok,0   ;Ok,0 = almeno un bit ่ settato

	btfsc	ServT1a,0
	bsf	Ok,0
	btfsc	ServT1a,1
	bsf	Ok,0
	btfsc	ServT1a,2
	bsf	Ok,0
	btfsc	ServT1a,3
	bsf	Ok,0
	btfsc	ServT1a,4
	bsf	Ok,0
	btfsc	ServT1a,5
	bsf	Ok,0
	btfsc	ServT1a,6
	bsf	Ok,0
	btfsc	ServT1a,7
	bsf	Ok,0

	btfsc	ServT2a,0
	bsf	Ok,0
	btfsc	ServT2a,1
	bsf	Ok,0
	btfsc	ServT2a,2
	bsf	Ok,0
	btfsc	ServT2a,3
	bsf	Ok,0
	btfsc	ServT2a,4
	bsf	Ok,0
	btfsc	ServT2a,5
	bsf	Ok,0
	btfsc	ServT2a,6
	bsf	Ok,0
	btfsc	ServT2a,7
	bsf	Ok,0

	btfsc	ServT3a,0
	bsf	Ok,0
	btfsc	ServT3a,1
	bsf	Ok,0
	btfsc	ServT3a,2
	bsf	Ok,0
	btfsc	ServT3a,3
	bsf	Ok,0
	btfsc	ServT3a,4
	bsf	Ok,0
	btfsc	ServT3a,5
	bsf	Ok,0
	btfsc	ServT3a,6
	bsf	Ok,0
	btfsc	ServT3a,7
	bsf	Ok,0

	;se Ok,0 vale ancora 0 -> nessun bit ่ settato = il servo vale 0
	btfss	Ok,0
	goto	errore

	;controllo che non valgano 255
	bcf	Ok,0   ;Ok,0 = almeno un bit ่ a zero

	btfss	ServT1a,0
	bsf	Ok,0
	btfss	ServT1a,1
	bsf	Ok,0
	btfss	ServT1a,2
	bsf	Ok,0
	btfss	ServT1a,3
	bsf	Ok,0
	btfss	ServT1a,4
	bsf	Ok,0
	btfss	ServT1a,5
	bsf	Ok,0
	btfss	ServT1a,6
	bsf	Ok,0
	btfss	ServT1a,7
	bsf	Ok,0

	btfss	ServT2a,0
	bsf	Ok,0
	btfss	ServT2a,1
	bsf	Ok,0
	btfss	ServT2a,2
	bsf	Ok,0
	btfss	ServT2a,3
	bsf	Ok,0
	btfss	ServT2a,4
	bsf	Ok,0
	btfss	ServT2a,5
	bsf	Ok,0
	btfss	ServT2a,6
	bsf	Ok,0
	btfss	ServT2a,7
	bsf	Ok,0

	btfss	ServT3a,0
	bsf	Ok,0
	btfss	ServT3a,1
	bsf	Ok,0
	btfss	ServT3a,2
	bsf	Ok,0
	btfss	ServT3a,3
	bsf	Ok,0
	btfss	ServT3a,4
	bsf	Ok,0
	btfss	ServT3a,5
	bsf	Ok,0
	btfss	ServT3a,6
	bsf	Ok,0
	btfss	ServT3a,7
	bsf	Ok,0

	;se Ok,0 vale ancora 0 -> tutti i bit sono settati = il servo vale 255
	btfss	Ok,0
	goto	errore

	;controllo che la differenza con i valori precedenti non sia superiore a 31
	bcf	Ok,0   ;Ok,0 = almeno un bit ่ settato

	;per preservare ServT1a trasferisco i valori in ServT1b	
	movf	ServT1a,0
	movwf	ServT1b

	;controllo con temp > servo
	movf	Servo01,0   ;Servo01 -> W (,0)
	subwf	ServT1b,1   ;ServT1b - W ->ServT1b (,1)
	
	;btfsc	ServT1b,5
	;bsf	Ok,0
	btfsc	ServT1b,6
	bsf	Ok,0
	btfsc	ServT1b,7
	bsf	Ok,0

	;se Ok,0 vale ancora 0 -> nessun bit ่ settato = il tmp-servo non ่ maggiore di 64 o tmp < Servo
	btfss	Ok,0
	goto	T1ok

	bcf	Ok,0   ;Ok,0 = almeno un bit ่ settato

	movf	ServT1a,0
	movwf	ServT1b

	;controllo con servo > tmp
	movf	ServT1b,0   ;ServT1b -> W (,0)
	subwf	Servo01,0   ;Servo01 - W -> W (,0)
	movwf	ServT1b     ;Servo01 - ServT1b -> ServT1b

	;btfsc	ServT1b,5
	;bsf	Ok,0
	btfsc	ServT1b,6
	bsf	Ok,0
	btfsc	ServT1b,7
	bsf	Ok,0

	;se Ok,0 vale ancora 0 -> nessun bit ่ settato = neanche il servo-tmp non ่ maggiore di 64
	btfsc	Ok,0
	goto	errore

T1ok

	;per preservare ServT2a trasferisco i valori in ServT2b	
	movf	ServT2a,0
	movwf	ServT2b

	;controllo con temp > servo
	movf	Servo02,0   ;Servo02 -> W (,0)
	subwf	ServT2b,1   ;ServT2b - W ->ServT2b (,1)
	
	btfsc	ServT2b,5
	bsf	Ok,0
	btfsc	ServT2b,6
	bsf	Ok,0
	btfsc	ServT2b,7
	bsf	Ok,0

	;se Ok,0 vale ancora 0 -> nessun bit ่ settato = il tmp-servo non ่ maggiore di 15 o tmp < Servo
	btfss	Ok,0
	goto	T2ok

	bcf	Ok,0   ;Ok,0 = almeno un bit ่ settato

	movf	ServT2a,0
	movwf	ServT2b

	;controllo con servo > tmp
	movf	ServT2b,0   ;ServT2b -> W (,0)
	subwf	Servo02,0   ;Servo02 - W -> W (,0)
	movwf	ServT2b     ;Servo02 - ServT2b -> ServT2b

	btfsc	ServT2b,5
	bsf	Ok,0
	btfsc	ServT2b,6
	bsf	Ok,0
	btfsc	ServT2b,7
	bsf	Ok,0

	;se Ok,0 vale ancora 0 -> nessun bit ่ settato = neanche il servo-tmp non ่ maggiore di 15
	btfsc	Ok,0
	goto	errore

T2ok

	;per preservare ServT3a trasferisco i valori in ServT3b	
	movf	ServT3a,0
	movwf	ServT3b

	;controllo con temp > servo
	movf	Servo03,0   ;Servo03 -> W (,0)
	subwf	ServT3b,1   ;ServT3b - W ->ServT3b (,1)
	
	btfsc	ServT3b,5
	bsf	Ok,0
	btfsc	ServT3b,6
	bsf	Ok,0
	btfsc	ServT3b,7
	bsf	Ok,0

	;se Ok,0 vale ancora 0 -> nessun bit ่ settato = il tmp-servo non ่ maggiore di 15 o tmp < Servo
	btfss	Ok,0
	goto	T3ok

	bcf	Ok,0   ;Ok,0 = almeno un bit ่ settato

	movf	ServT3a,0
	movwf	ServT3b

	;controllo con servo > tmp
	movf	ServT3b,0   ;ServT3b -> W (,0)
	subwf	Servo03,0   ;Servo03 - W -> W (,0)
	movwf	ServT3b     ;Servo03 - ServT3b -> ServT3b

	btfsc	ServT3b,5
	bsf	Ok,0
	btfsc	ServT3b,6
	bsf	Ok,0
	btfsc	ServT3b,7
	bsf	Ok,0

	;se Ok,0 vale ancora 0 -> nessun bit ่ settato = neanche il servo-tmp non ่ maggiore di 15
	btfsc	Ok,0
	goto	errore

T3ok



	;tutti i test passati: carica i Servo

	movf	ServT1a,0
	movwf	Servo01
	movf	ServT2a,0
	movwf	Servo02
	movf	ServT3a,0
	movwf	Servo03
	movf	ServT4a,0
	movwf	Servo04

	;se avanti ่ attivo setta Autostp a 3
	btfsc	Servo04,2
	movlw	3
	;se indietro ่ attivo setta Autostp a 3
	btfsc	Servo04,3
	movlw	3
	;se destra ่ attivo setta Autostp a 2
	btfsc	Servo04,4
	movlw	2
	;se sinistra ่ attivo setta Autostp a 2
	btfsc	Servo04,5
	movlw	2

	movwf	Autostp


	bsf	PORTA,3 ;accende il led verde
        clrwdt                  ;Clear watch dog timer

	goto	cont
	

errore
	bcf	PORTA,3 ;spegne il led verde

cont
	return



;attendi clock-up
AUp
	btfss	PORTA,0
	goto	AUp	

	return

;attendi clock-down
ADw
	btfsc	PORTA,0
	goto	ADw

	return

;LoadServo
LoadServo
	clrf	Servo

	btfss	PORTA,0
	bcf	Servo,0
	btfsc	PORTA,0
	bsf	Servo,0
	btfsc	PORTA,0
	incf	Verif2,F   ;incrementa solo in presenza di un "1"
	call	DelayR


	btfss	PORTA,0
	bcf	Servo,1
	btfsc	PORTA,0
	bsf	Servo,1
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR


	btfss	PORTA,0
	bcf	Servo,2
	btfsc	PORTA,0
	bsf	Servo,2
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR


	btfss	PORTA,0
	bcf	Servo,3
	btfsc	PORTA,0
	bsf	Servo,3
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR


	btfss	PORTA,0
	bcf	Servo,4
	btfsc	PORTA,0
	bsf	Servo,4
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR


	btfss	PORTA,0
	bcf	Servo,5
	btfsc	PORTA,0
	bsf	Servo,5
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR


	btfss	PORTA,0
	bcf	Servo,6
	btfsc	PORTA,0
	bsf	Servo,6
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR


	btfss	PORTA,0
	bcf	Servo,7
	btfsc	PORTA,0
	bsf	Servo,7
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR

	return


;carica i 2 bit per la pinza
LoadPinza
	btfss	PORTA,0
	bcf	ServT4a,0
	btfsc	PORTA,0
	bsf	ServT4a,0
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR


	btfss	PORTA,0
	bcf	ServT4a,1
	btfsc	PORTA,0
	bsf	ServT4a,1
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR
	
	return

;carica i 2 bit avanti/indietro
LoadAvInd
	btfss	PORTA,0
	bcf	ServT4a,2
	btfsc	PORTA,0
	bsf	ServT4a,2
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR


	btfss	PORTA,0
	bcf	ServT4a,3
	btfsc	PORTA,0
	bsf	ServT4a,3
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR
	
	return

;carica i 2 bit destra/sinistra
LoadDxSx
	btfss	PORTA,0
	bcf	ServT4a,4
	btfsc	PORTA,0
	bsf	ServT4a,4
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR

	btfss	PORTA,0
	bcf	ServT4a,5
	btfsc	PORTA,0
	bsf	ServT4a,5
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR
	
	btfss	PORTA,0
	bcf	ServT4a,6
	btfsc	PORTA,0
	bsf	ServT4a,6
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR
	
	return

;carica 1 bit FastServo
LoadFS
	btfss	PORTA,0
	bcf	ServT4a,7
	btfsc	PORTA,0
	bsf	ServT4a,7
	btfsc	PORTA,0
	incf	Verif2,F
	call	DelayR

	return


;carica i 6 bit per la verifica
LoadVerif
	btfss	PORTA,0
	bcf	Verif1,0
	btfsc	PORTA,0
	bsf	Verif1,0
	call	DelayR

	btfss	PORTA,0
	bcf	Verif1,1
	btfsc	PORTA,0
	bsf	Verif1,1
	call	DelayR

	btfss	PORTA,0
	bcf	Verif1,2
	btfsc	PORTA,0
	bsf	Verif1,2
	call	DelayR

	btfss	PORTA,0
	bcf	Verif1,3
	btfsc	PORTA,0
	bsf	Verif1,3
	call	DelayR

	btfss	PORTA,0
	bcf	Verif1,4
	btfsc	PORTA,0
	bsf	Verif1,4
	call	DelayR

	btfss	PORTA,0
	bcf	Verif1,5
	btfsc	PORTA,0
	bsf	Verif1,5
	call	DelayR

	return

;Incremeta/decrementa servo1
ARServ1
	btfsc	Servo04,7	;se fast servo ่ inserito salta a SetSer
	goto	SetSer

	movf	Servo01,0	;Servo01 -> W
	subwf	ServAR1,w	;Servo01 - ServAR1
	jg	maggiore	;salta se Servo01 > ServAR1
	jl	minore
	goto usc
SetSer
	Movff	Servo01,ServAR1
	goto	usc
maggiore
	incf	ServAR1,F
	goto usc
minore
	decf	ServAR1,F
usc

	return


;Incremeta/decrementa servo2
ARServ2
	btfsc	Servo04,7	;se fast servo ่ inserito salta a SetSer
	goto	SetSer2

	movf	Servo02,0	;Servo02 -> W
	subwf	ServAR2,w	;Servo02 - ServAR2
	jg	maggior2	;salta se Servo02 > ServAR2
	jl	minore2
	goto usc2
SetSer2
	Movff	Servo02,ServAR2
	goto	usc2
maggior2
	incf	ServAR2,F
	goto usc2
minore2
	decf	ServAR2,F
usc2
	return


;Incremeta/decrementa servo3
ARServ3
	btfsc	Servo04,7	;se fast servo ่ inserito salta a SetSer
	goto	SetSer3

	movf	Servo03,0	;Servo03 -> W
	subwf	ServAR3,w	;Servo03 - ServAR3
	jg	maggior3	;salta se Servo03 > ServAR3
	jl	minore3
	goto usc3
SetSer3
	Movff	Servo03,ServAR3
	goto	usc3
maggior3
	incf	ServAR3,F
	goto usc3
minore3
	decf	ServAR3,F
usc3
	return


        END