;**********************************************************************
;
;	CONTROLE SYNCHRONISE DE 8 SERVOS DE MODELISME 
;
;	AVEC LIAISON SERIE 19200 BAUDS 8 BITS 1 STOP (CLASSIQUE !)
;
;	LES PULSES SONT ENVOYES AUX 8 SERVOS EN MEME TEMPS,
;	CE QUI LAISSE 17.5ms AU MAITRE POUR ENVOYER UNE OU PLUSIEURS CONSIGNES
;
;	CHAQUE CONSIGNE EST CONSTITUEE DE 2 OCTETS
;	1ER OCTET : COMMANDE OU ADRESSE SERVO (0 A 7) , 2EME OCTET : POSITION (1 A 255)
;	SI POSITION = 0 , LE SERVO CORRESPONDANT EST DEVALIDE (HORS COUPLE)
;
;	COMMANDE 8 : TOUS LES SERVOS AVEC LA MËME CONSIGNE (UTIL POUR TOUS LES ETEINDRE)
;	COMMANDE 9 : REGLE L'OFFSET GENERAL (DE TOUS LES SERVOS), VALEUR NORMALE = D'73'
;
;
;	LA VITESSE DE TRANSMISSION DE 19200 PERMET LARGEMENT D'ENVOYER A LA SUITE
;		8 CONSIGNES POUR LES 8 SERVOS ENTRE 2 TRAMES DE PULSES DONC LES
;		8 SERVOS REAGIRONT EN MEME TEMPS DES LA TRAME SUIVANTE
;
;**********************************************************************
;
;
;		16F628	Quartz 20Mhz
;

;**********************************************************************

	list      p=16F628
	#include <p16F628.inc>

; Pas de code protégé
; Pas de watchdog timer
; Délai de démarrage
; Horloge HS.

	__CONFIG   _CP_OFF & _WDT_OFF & _PWRTE_ON & _HS_OSC & _BODEN_OFF & _LVP_OFF & _MCLRE_OFF

;		DEFINITION DES VARIABLES

WTEMP		EQU	0x20
MEM1		EQU	0x21
MEM2		EQU	0x22
MEMOA		EQU	0x23
MEMOB		EQU	0x2C
POS0		EQU	0x24
POS1		EQU	0x25
POS2		EQU	0x26
POS3		EQU	0x27
POS4		EQU	0x28
POS5		EQU	0x29
POS6		EQU	0x2A
POS7		EQU	0x2B
CPT1		EQU	0X2D
TMP1		EQU	0X2E
ADRSERVO	EQU	0x2F
VALSERVO	EQU	0x30
TMP2		EQU	0x31
OFFSET		EQU	0x32
ADRCARTE	EQU	0x33


;**********************************************************************


;**********************************************************************
		ORG     0x000           ; Vecteur reset
  		GOTO    main            ; Début du programme

;**********************************************************************
;
;    Interruption 
;
;**********************************************************************

	ORG     0x004           ; Vecteur d'interruption
	MOVWF	WTEMP

	MOVLW	H'3B'
	MOVWF	TMR1H		; Init du Timer
	MOVLW	H'AD'
	MOVWF	TMR1L

	BCF	MEMOB,3		; ARRET D'AUTORISATION DE RECEPTION CONSIGNE
	MOVF	MEMOB,W		; EN METTANT RB3 A 0
	MOVWF	PORTB

	BCF 	PIR1,TMR1IF	; On efface l'interruption TimeR	

ANALYSE

	MOVF	POS0,W
	BTFSC	STATUS,Z
	GOTO	SUITE1
	BSF	MEMOB,4

SUITE1	MOVF	POS1,W
	BTFSC	STATUS,Z
	GOTO	SUITE2
	BSF	MEMOB,5

SUITE2	MOVF	POS2,W
	BTFSC	STATUS,Z
	GOTO	SUITE3
	BSF	MEMOB,6

SUITE3	MOVF	POS3,W
	BTFSC	STATUS,Z
	GOTO	SUITE4
	BSF	MEMOB,7

SUITE4	MOVF	POS4,W
	BTFSC	STATUS,Z
	GOTO	SUITE5
	BSF	MEMOA,0

SUITE5	MOVF	POS5,W
	BTFSC	STATUS,Z
	GOTO	SUITE6
	BSF	MEMOA,1

SUITE6	MOVF	POS6,W
	BTFSC	STATUS,Z
	GOTO	SUITE7
	BSF	MEMOA,3

SUITE7	MOVF	POS7,W
	BTFSC	STATUS,Z
	GOTO	SUITE
	BSF	MEMOA,2	

SUITE	MOVF	MEMOA,W
	MOVWF	PORTA
	MOVF	MEMOB,W
	MOVWF	PORTB


TEMPO				; DUREE 415.2us
	MOVF	OFFSET,W		; TEMPO DE N x ( 5 + T2) + 4 CYCLES
	MOVWF	TMP1
T1	DECF	TMP1,F

T2	MOVLW	D'10'		; TEMPO DE N x 3 + 2 CYCLES
	MOVWF	TMP2
	DECFSZ	TMP2,F
	GOTO	$-1

	MOVF	TMP1,F
	BTFSS	STATUS,Z
	GOTO	T1
	NOP
	NOP
	NOP
	NOP

PULSE				; ANALYSE DES POSITIONS ET ARRET DES PULSES CONCERNES

	MOVF	CPT1,W
	SUBWF	POS0,W
	BTFSC	STATUS,Z
	BCF	MEMOB,4

	MOVF	CPT1,W
	SUBWF	POS1,W
	BTFSC	STATUS,Z
	BCF	MEMOB,5

	MOVF	CPT1,W
	SUBWF	POS2,W
	BTFSC	STATUS,Z
	BCF	MEMOB,6

	MOVF	CPT1,W
	SUBWF	POS3,W
	BTFSC	STATUS,Z
	BCF	MEMOB,7

	MOVF	CPT1,W
	SUBWF	POS4,W
	BTFSC	STATUS,Z
	BCF	MEMOA,0

	MOVF	CPT1,W
	SUBWF	POS5,W
	BTFSC	STATUS,Z
	BCF	MEMOA,1

	MOVF	CPT1,W
	SUBWF	POS6,W
	BTFSC	STATUS,Z
	BCF	MEMOA,3

	MOVF	CPT1,W
	SUBWF	POS7,W
	BTFSC	STATUS,Z
	BCF	MEMOA,2

	MOVF	MEMOA,W
	MOVWF	PORTA
	MOVF	MEMOB,W
	MOVWF	PORTB

	INCFSZ	CPT1,F
	GOTO	PULSE

	CLRF	CPT1

	BSF	MEMOB,3		; AUTORISATION DE RECEPTION CONSIGNE
	MOVF	MEMOB,W		; EN METTANT RB3 A 1
	MOVWF	PORTB
	

	MOVF	WTEMP,W
	RETFIE






;**********************************************************************
;
;    Programme Main
;
;**********************************************************************

main


; Initialisation des ports d'E/S

	MOVlw	B'00000000'
	MOVWF	ADRCARTE	; Adresse du PIC entre 0 et 3 (les 2 bit de poids fort)

	CLRF	MEM1
	CLRF	MEM2
	CLRF	CPT1
	CLRF	MEMOA
	CLRF	MEMOB
	CLRF	PORTB		; Initialise le port B
	CLRF 	PORTA		; Initialise le port A
	MOVLW	D'73'
	MOVWF	OFFSET
	MOVLW	D'00'
	MOVWF	POS0		; INIT DE TOUTES LES CONSIGNES
	MOVLW	D'48'
	MOVWF	POS1
	MOVLW	D'80'
	MOVWF	POS2
	MOVLW	D'112'
	MOVWF	POS3
	MOVLW	D'128'
	MOVWF	POS4
	MOVLW	D'176'
	MOVWF	POS5
	MOVLW	D'208'
	MOVWF	POS6
	MOVLW	D'240'
	MOVWF	POS7

	BSF	STATUS, RP0	; Bank 1 sélectionnée

	MOVLW	B'00000011'	; Port RB2-7 en sortie
	MOVWF	TRISB		; /------------ ATTENTION RB2=TX DE L'USART ET RB1=RX DE L'USART

	MOVLW	B'00000000'	; Port A en sortie
	MOVWF	TRISA		;	/

	MOVLW	B'10000000'	; PAS DE PULLUP SUR PORTB
	MOVWF	OPTION_REG

	BCF 	STATUS,	RP0	; Bank 0 sélectionnée
	

; INIT DU TIMER1 AVEC INTERRUPTION

	MOVLW	H'3B'
	MOVWF	TMR1H		; Init du Timer
	MOVLW	H'AD'
	MOVWF	TMR1L

	BCF	PIR1,TMR1IF	; EFFACEMENT DU FLAG INTERRUPTION TIMER1

	MOVLW	B'00010000'	; PRESCALER TIMER1 A 1:2
	MOVWF	T1CON
	BSF	STATUS,RP0
	BSF	PIE1,TMR1IE	; AUTORISATION INTERRUPTION PAR TIMER1
	BCF	STATUS,RP0	
	MOVLW   B'11000000'     ; Active les Interruptions GENERALES ET PERIPHERIQUES
	MOVWF   INTCON          ;       /

; INIT DE L'USART

	BSF	STATUS,RP0
	MOVLW	B'00000100'
	MOVWF	TXSTA
	MOVLW	D'64'		; VITESSE A 19200 BAUDS
	MOVWF	SPBRG
;	BCF	PIE1,RCIE	; DESACTIVE INTERRUPTION EN RECEPTION
	BCF	STATUS,RP0
	MOVLW	B'10010000'
	MOVWF	RCSTA

	BSF	T1CON,TMR1ON	; MISE EN ROUTE DU TIMER1


; LE 1ER OCTET DOIT ETRE EGAL A D'255'
;------------------------------------NE SERT PLUS------------------------------
LOOP
;	BTFSS	PIR1,RCIF
;	GOTO	LOOP
;	MOVF	RCREG,0
;	MOVWF	MEM1

;	MOVLW	D'255'
;	SUBWF	MEM1,W
;	BTFSS	STATUS,Z
;	GOTO	LOOP
;------------------------------------------------------------------------------
; LE 2EME OCTET CONTIENT
;		L'ADRESSE DU PIC : BIT 6 ET 7
;		RIEN : BIT 4 ET 5
;		COMMANDE ET/OU N° DE SERVO A COMMANDER : BIT 0 A 3

LOOP2
	BTFSS	PIR1,RCIF
	GOTO	LOOP2
	MOVF	RCREG,W
	MOVWF	MEM1

	ANDLW	B'11000000'
	SUBWF	ADRCARTE,W
	BTFSS	STATUS,Z	;TEST DE L'ADRESSE
	GOTO	LOOP

	MOVF	MEM1,W		; MEMORISATION DE L'ADRESSE DU SERVO A POSITIONNER
	MOVWF	ADRSERVO

; LE 3EME OCTET EST LA CONSIGNE DE POSITION

LOOP3
	BTFSS	PIR1,RCIF
	GOTO	LOOP3
	MOVF	RCREG,0
	MOVWF	VALSERVO		

; MEMORISATION DE LA CONSIGNE A L'ENDROIT VOULU

	MOVLW	B'00001111'
	ANDWF	ADRSERVO,F

	MOVLW	D'10'
	SUBWF	ADRSERVO,W
	BTFSC	STATUS,C
	GOTO	FIN

	MOVLW	D'8'		; LA COMMANDE 8 ENVOIE LA MEME CONSIGNE A TOUS LES SERVOS
	SUBWF	ADRSERVO,W
	BTFSC	STATUS,Z
	GOTO	TOUSSERVOS
	
	MOVLW	D'9'		; LA COMMANDE 9 PERMET D'AJUSTER L'OFFSET
	SUBWF	ADRSERVO,W
	BTFSC	STATUS,Z
	GOTO	MODOFFSET
	
	MOVLW	POS0		
	ADDWF	ADRSERVO,W
	MOVWF	FSR		; CHARGER LE FSR AVEC L'ADRESSE A ACCEDER (POS0+ADRSERVO)
	MOVF	VALSERVO,W
	MOVWF	INDF		; CHARGER LA VALEUR VALSERVO A CETTE ADRESSE

	NOP
	GOTO	LOOP

TOUSSERVOS
	MOVF	VALSERVO,W
	MOVWF	POS0
	MOVWF	POS1
	MOVWF	POS2
	MOVWF	POS3
	MOVWF	POS4
	MOVWF	POS5
	MOVWF	POS6
	MOVWF	POS7

FIN
	NOP
	GOTO	LOOP

MODOFFSET
	MOVF	VALSERVO,W
	MOVWF	OFFSET
	GOTO	LOOP



;**********************************************************************
;
;    Fin
;
;**********************************************************************

	END