;**************************************************************************
;* PROGRAMMA OM 16 SERVO'S AAN TE STUREN MET SCHAKELAAR EN POTMETER       *
;*                                                                        *
;* Geschreven door Geert Giebens voor PIC 16F887 microcontroller          *
;*                                                                        *
;* Datum: 27 december 2012         Versie PIC16F887_SERVO_SCHAKKELAAR_1.1 *
;*                                                                        *
;* Filenaam: 16_servo_met_schakelaar_v1p1.asm                             *
;*								          *
;* Aanpassingen versie 1.1:					          * 
;*    Bij INIT alle uitgangen naar 0                   	 		  *
;*                        					          *
;*                         _____________________		          *
;*      schakelaar servo 1| 1  RE3 *  *  RB7 40 | puls servo 2            *
;*      schakelaar servo 2| 2  RA0  **   RB6 39 | puls servo 3            *
;*      schakelaar servo 3| 3  RA1       RB5 38 | puls servo 4            *
;*      schakelaar servo 4| 4  RA2       RB4 37 | puls servo 5            *
;*      schakelaar servo 5| 5  RA3   I   RB3 36 | puls servo 6            *
;*      schakelaar servo 6| 6  RA4   C   RB2 35 | puls servo 7            *
;*      schakelaar servo 7| 7  RA5       RB1 34 | puls servo 8            *
;*      schakelaar servo 8| 8  RE0   P   RB0 33 | DCC of MM in   v2p1.asm *
;*           puls servo 1 | 9  RE1   I       32 | +5V		 	  *
;*        ingang potmeter | 10 RE2   C       31 | Massa	 	          *
;*	    	      +5V | 11           RD7 30 | ingang keuze rechts     *
;*	    	    Massa | 12       1   RD6 29 | ingang keuze links      *
;*      schakelaar servo 9| 13 RA7   6   RD5 28 | puls servo 9            *
;*     schakelaar servo 10| 14 RA6   F   RD4 27 | puls servo 10           *
;*     schakelaar servo 11| 15 RC0   8   RC7 26 | Puls Servo 11           *
;*     schakelaar servo 12| 16 RC1   8   RC6 25 | Puls Servo 12           *
;*     schakelaar servo 13| 17 RC2   7   RC5 24 | Puls Servo 13           *
;*     schakelaar servo 14| 18 RC3       RC4 23 | Puls Servo 14           *
;*     schakelaar servo 15| 19 RD0       RD3 22 | Puls Servo 15           *
;*     schakelaar servo 16| 20 RD1       RD2 21 | Puls Servo 16           *
;*                         _____________________			  *
;*								          *
;*								          *								    
;**************************************************************************



;********************************************************************
;************ µCONTROLLER AFHANKELIJKE INSTELLINGEN *****************
;********************************************************************

	list		p=16f887	; list directive to define processor
	#include	<p16f887.inc>	; processor specific variable definitions

	__CONFIG    _CONFIG1, _LVP_OFF & _FCMEN_ON & _IESO_OFF & _BOR_OFF & _CPD_OFF & _CP_OFF & _MCLRE_OFF & _PWRTE_ON & _WDT_OFF & _INTRC_OSC_NOCLKOUT
	__CONFIG    _CONFIG2, _WRT_OFF & _BOR21V

	errorlevel -302, -306


;********************************************************************
;************ CONSTANTEN ********************************************
;********************************************************************

				;Inhoud FLAG register:

FLAG20ms		EQU 0	;=1 na 20ms
FLAGT2			EQU 1	;=1 dan timer 2 INT: 0-2,56ms (servopuls voorbij) 
FLAGPSZO		EQU 3	;geeft aan dat Persoonlijke Servo vertraging een Zero Overgang heeft gehad
FLANK			EQU 4   ;FLANK ingang van 0 --> 1
INGANG			EQU 5	;toestand ingang
MINPULS			EQU .50	;minimale waarde servopuls 50/100 = 0,5ms
MAXPULS			EQU .250 ;maximale waarde servopuls 250/100 = 2,5ms

	#define MIN_KEUZE PORTD,7
	#define MAX_KEUZE PORTD,6

	#define SCHAKELAAR1 PORTE,3
	#define SCHAKELAAR2 PORTA,0
	#define SCHAKELAAR3 PORTA,1
	#define SCHAKELAAR4 PORTA,2
	#define SCHAKELAAR5 PORTA,3
	#define SCHAKELAAR6 PORTA,4
	#define SCHAKELAAR7 PORTA,5
	#define SCHAKELAAR8 PORTE,0
	#define SCHAKELAAR9 PORTA,7
	#define SCHAKELAAR10 PORTA,6
	#define SCHAKELAAR11 PORTC,0
	#define SCHAKELAAR12 PORTC,1
	#define SCHAKELAAR13 PORTC,2
	#define SCHAKELAAR14 PORTC,3
	#define SCHAKELAAR15 PORTD,0
	#define SCHAKELAAR16 PORTD,1

	#define SERVO1 PORTE,1
	#define SERVO2 PORTB,7
	#define SERVO3 PORTB,6
	#define SERVO4 PORTB,5
	#define SERVO5 PORTB,4
	#define SERVO6 PORTB,3
	#define SERVO7 PORTB,2
	#define SERVO8 PORTB,1
	#define SERVO9 PORTD,5
	#define SERVO10 PORTD,4
	#define SERVO11 PORTC,7
	#define SERVO12 PORTC,6
	#define SERVO13 PORTC,5
	#define SERVO14 PORTC,4
	#define SERVO15 PORTD,3
	#define SERVO16 PORTD,2

	

;*******************************************************************
;************ VARIABELEN ********************************************
;********************************************************************



;de volgende MINSERVO MAXSERVO en TIMSERVO variabelen moeten in deze volgorde en eerst staan in ASM-program i.v.m. opslag en opvragen EEPROM !!!

	cblock 0x20
MINSERVO		;Array[16] ingestelde Min waarde servo zoals opgeslagen in EEPROM
	endc
	cblock 0x30
MAXSERVO		;Array[16] ingestelde Max waarde servo zoals opgeslagen in EEPROM
	endc
	CBLOCK 0X40
TIMSERVO		;Array[16] looptijd servo (vertraging) zoals opgeslagen in EEPROM
	ENDC
	CBLOCK 0x50
PULS			;Array[16] huidige waarde pulsduur servo
	endc
	cblock 0x60
TELVERTR		;Array[16] hulpteller vertraging servo
	endc
	cblock 0x70
HULPREG				;kan voor diverse doeleinden gebruikt worden
FLAG				;bits van FLAG register geven diverse toestanden weer (zie bij constanten)
SCHFF1				;vorige toestand schakelaar ingang 1-8
SCHFF2				;vorige toestand schakelaar ingang 9-16
VERTRAGING			;Berekende vertraging uit ADC register
SERVOTIJD			;Berekende tijd servo uit ADC register	
		
STATUS_TEMP			;tijdelijke opslag tijdens Interruptroutine
W_TEMP
PCLATH_TEMP			; save PCLATH
	
	endc


;********************************************************************
;*******  MACRO SERVO ***********************************************
;********************************************************************

SERVO MACRO  M_MINSERVO, M_MAXSERVO, M_TIMSERVO, M_PULS,M_TELVERT,M_SCHFF,M_FFBIT,M_PORTIN,M_BITIN,M_PORTOUT,M_BITOUT

					
	LOCAL  M_END,M_SPR10,M_SPR20, M_SPR25,M_SPR27,M_SPR30,M_SPR31,M_SPR40,M_SPR35,M_SPR23,M_SPR50,M_SPR55,M_SPR00, M_SPR42

					;EERST TOESTAND SCHAKELAAR ACHTERHALEN o.a. voor positieve FLANK detectie

	BCF FLAG,INGANG			;clear eerst de flag's
	BCF FLAG,FLANK
	BTFSS M_PORTIN,M_BITIN		;is schakelaar '1'
	GOTO M_SPR50
	BSF FLAG,INGANG			;  JA: set flag INGANG
	BTFSC M_SCHFF,M_FFBIT		;      is vorige toestand schakelaar '0'
	GOTO M_SPR00			;        NEE: ga naar INSTELLING VERTRAGING
	BSF FLAG,FLANK			;        JA: SET flag FLANK 
	BSF M_SCHFF,M_FFBIT		;            SET vorige toestand schakelaar
	GOTO M_SPR00
	
	
M_SPR50 BCF M_SCHFF,M_FFBIT		;  NEE: clear vorige toestand schakelaar
	
					;INSTELLEN VERTRAGING? 

M_SPR00	BTFSC MAX_KEUZE		;is ingang keuze max '0'
	GOTO M_SPR10			;  NEE: ga naar TEST SERVO MIN INSTELLING
	BTFSC MIN_KEUZE			;  JA: is ingang keuze min '0'
	GOTO M_SPR20			;        NEE: ga naar INSTELLEN SERVOPULS MAX
	BTFSS FLAG,FLANK		;        JA: is er een positieve flank
	GOTO M_END			;                 NEE: ga naar einde macro
	MOVF VERTRAGING,w		;             JA: Laad vertraging in array TIMSERVO	
	MOVWF M_TIMSERVO	  
	MOVLW LOW M_TIMSERVO		;             en opslaan in EEPROM en einde MACRO	
	banksel EEADR		
	MOVWF EEADR 		
	banksel M_TIMSERVO
	MOVF M_TIMSERVO,w
	banksel EEDAT
	MOVWF EEDAT
	GOTO M_SPR23

					;INSTELLEN SERVOPULS MIN?
	
M_SPR10 BTFSC MIN_KEUZE			;is ingang keuze min '0'
	GOTO M_SPR30			;  NEE: ga naar TEST SERVOPULS <> SERVOPULS MAX of SERVOPULS MIN
	BTFSS FLAG,INGANG		;  JA: is schakelaar '1'
	GOTO M_SPR25			;        NEE: GA NAAR SERVO UITGANG AANSTUREN
	BTFSS FLAG,FLANK		;        JA: is vorige toestand schakelaar '0' (flank)
	GOTO M_END			;              NEE: ga naar einde macro
	MOVF SERVOTIJD,w		;      	       JA: laad servotijd in array MINSERVO
	MOVWF M_MINSERVO
	MOVLW LOW M_MINSERVO		;                  en opslaan in EEPROM en einde MACRO	
	banksel EEADR		
	MOVWF EEADR 		
	banksel M_MINSERVO
	MOVF M_MINSERVO,w
	banksel EEDAT
	MOVWF EEDAT
	GOTO M_SPR23

					;INSTELLEN SERVOPULS MAX?

M_SPR20 BTFSC MAX_KEUZE			;is ingang keuze max '0'
	GOTO M_SPR30			;  NEE: ga naar TEST SERVOPULS <> SERVOPULS MAX of SERVOPULS MIN
	BTFSS FLAG,INGANG		;  JA: is schakelaar '1'
	GOTO M_SPR25			;        NEE: GA NAAR SERVO UITGANG AANSTUREN
	BTFSS FLAG,FLANK		;        JA: is vorige toestand schakelaar '0' (flank)
	GOTO M_END			;              NEE: ga naar einde macro
	MOVF SERVOTIJD,w		;      	       JA: laad servotijd in array MAXSERVO
	MOVWF M_MAXSERVO

	MOVLW LOW M_MAXSERVO		;                   en opslaan in EEPROM en einde MACRO	
	banksel EEADR		
	MOVWF EEADR 		
	banksel M_MAXSERVO
	MOVF M_MAXSERVO,w
	banksel EEDAT
	MOVWF EEDAT
M_SPR23	
	banksel EECON1			;gemeenschapelijk deel verder verloop opslag EEPROM, uitleg zie µC manual
	BCF EECON1, EEPGD 	
	BSF EECON1, WREN 	
	BCF INTCON, GIE 	
	BTFSC INTCON, GIE 	
	GOTO $-2
	MOVLW 0x55 
	MOVWF EECON2 		
	MOVLW 0xAA 
	MOVWF EECON2 		
	BSF EECON1, WR 		
	BSF INTCON, GIE 	
	BCF EECON1, WREN	
	banksel STATUS
	GOTO M_END
					;SERVO UITGANG AANSTUREN BIJ INSTELLING SERVO MIN of MAX

M_SPR25	MOVF SERVOTIJD,w		;Waarde SERVOTIJD  in Timer 2
	banksel PR2			;RAM Bankselectie 
	MOVWF PR2
	banksel TMR2
	CLRF TMR2
	BSF T2CON,TMR2ON		;Start T2
	banksel 0
	BSF M_PORTOUT,M_BITOUT		;Uitgang servo-->1
	BCF FLAG,FLAGT2			;FLAGT2-->0
M_SPR27	BTFSS FLAG,FLAGT2		;is servopuls voorbij
	GOTO M_SPR27			;  NEE: verder wachten
	BCF M_PORTOUT,M_BITOUT		;  JA: uitgang servo-->0 en einde macro
	GOTO M_END

					;SERVOPULS <> SERVOPULS MAX of SERVOPULS MIN

M_SPR30 BCF FLAG, FLAGPSZO
	DECFSZ M_TELVERT,f		;eerst persoonlijke servo vertraging aanpassen
	GOTO M_SPR31
	MOVF M_TIMSERVO,w
	MOVWF M_TELVERT

	BSF FLAG,FLAGPSZO		;is er een overgang geweest geeft dit dan aan via een flag

M_SPR31 BTFSS FLAG,INGANG		;is schakelaar '1'
	GOTO M_SPR42			;  

					;  JA: VERWERKING INDIEN SCHAKELAAR '1'
	MOVF M_PULS,w
	SUBWF M_MINSERVO,w	
	BTFSC STATUS,C			;      is servoPULS groter dan MINPULS
	GOTO M_END			;        NEE: einde macro
	MOVF M_PULS,w		;	 	 JA: Waarde servoPULS  in Timer 2
	banksel PR2			;             RAM Bankselectie 
	MOVWF PR2
	banksel TMR2
	CLRF TMR2
	BSF T2CON,TMR2ON		;             Start T2
	banksel 0
	BSF M_PORTOUT,M_BITOUT		;             Uitgang servo-->1
	BCF FLAG,FLAGT2			;             FLAGT2-->0
M_SPR35	BTFSS FLAG,FLAGT2		;             is servopuls voorbij
	GOTO M_SPR35			;               NEE: verder wachten
	BCF M_PORTOUT,M_BITOUT		;               JA: uitgang servo-->0
	BTFSS FLAG,FLAGPSZO		;		    is er een persoonlijke vertraging overgang geweest
	GOTO M_END			;		      NEE: einde macro
	DECF M_PULS,f			;		      JA: verlaag puls en einde macro
	GOTO M_END

M_SPR42					;  NEE: VERWERKING INDIEN SCHAKELAAR '0'
	MOVF M_MAXSERVO,w
	SUBWF M_PULS,w	
	BTFSC STATUS,C			;      is servoPULS kleiner dan MAXPULS
	GOTO M_END			;        NEE: einde macro
	MOVF M_PULS,w		;	 	 JA: Waarde servoPULS  in Timer 2
	banksel PR2			;             RAM Bankselectie 
	MOVWF PR2
	banksel TMR2
	CLRF TMR2
	BSF T2CON,TMR2ON
	banksel 0			;             Start T2
	BCF FLAG,FLAGT2			;             FLAGT2-->0
	BSF M_PORTOUT,M_BITOUT		;             Uitgang servo-->1
M_SPR40	BTFSS FLAG,FLAGT2		;             is servopuls voorbij
	GOTO M_SPR40			;               NEE: verder wachten
	BCF M_PORTOUT,M_BITOUT		;               JA: uitgang servo-->0
	BTFSS FLAG,FLAGPSZO		;		    is er een persoonlijke vertraging overgang geweest
	GOTO M_END			;		      NEE: einde macro
	INCF M_PULS,f			;		      JA: verhoog puls en einde macro

M_END
	ENDM


;********************************************************************
;*******  MACRO INITPULS ********************************************
;********************************************************************

INITPULS MACRO M2_PORTIN, M2_PORTBIT, M2_PULS, M2_MINSERVO, M2_MAXSERVO, M2_TIMSERVO, M2_TELVERTR, M2_OUT, M2_OUTBIT

	LOCAL M2_END, M2_SPR1

	MOVF M2_MAXSERVO,w		;afhankelijk stand schakelaar var PULS servo instellen
	MOVWF M2_PULS
	BTFSS M2_PORTIN,M2_PORTBIT
	goto M2_SPR1
	MOVF M2_MINSERVO,w
	MOVWF M2_PULS
M2_SPR1 MOVF M2_TIMSERVO,w
	MOVWF M2_TELVERTR

	BCF M2_OUT,M2_OUTBIT		;clear SERVO uitgang
M2_END

	ENDM


;********************************************************************
;*******  START ASSEMBLER CODE **************************************
;********************************************************************

		ORG     0x0000    	; processor reset vector is altijd adres 0000

		clrf PCLATH
		pagesel MAIN	
  		GOTO    MAIN        	; ga naar begin van program


;********************************************************************
;******************* INTERRUPT SERVICE ROUTINE  *********************
;********************************************************************

	     	ORG    0x0004		;startadres interrupt code is altijd adres 0004
       
ISR     	
		MOVWF W_TEMP 		;Copy W to TEMP register
		SWAPF STATUS,W 		;Swap status to be saved into W
		CLRF STATUS 		;bank 0, regardless of current bank, Clears IRP,RP1,RP0
		MOVWF STATUS_TEMP 	;Save status to bank zero STATUS_TEMP register
		MOVF PCLATH,W 		;Only required if using pages 1, 2 and/or 3
		MOVWF PCLATH_TEMP 	;Save PCLATH into W
		CLRF PCLATH 		;Page zero, regardless of current page


		BTFSS	PIR1,TMR2IF	;TIMER 2 INTERRUPT?
		GOTO ISR_SPR1
		BCF T2CON,TMR2ON	;stop timer2
		BSF FLAG,FLAGT2		;geef aan via FLAGT2=1 dat servopuls voorbij is
		BCF PIR1,TMR2IF		;wis interruptflag

END_ISR		MOVF PCLATH_TEMP, W 	;Restore PCLATH
		MOVWF PCLATH		;Move W into PCLATH
		SWAPF STATUS_TEMP,W 	;Swap STATUS_TEMP register into W
		MOVWF STATUS 		;Move W into STATUS register
		SWAPF W_TEMP,F 		;Swap W_TEMP
		SWAPF W_TEMP,W 		;Swap W_TEMP into W
       		RETFIE


ISR_SPR1 	BTFSS INTCON,T0IF	;TIMER 0 INTERRUPT?
		GOTO END_ISR
		BCF INTCON,T0IF
		BSF FLAG,FLAG20ms
		GOTO END_ISR



;********************************************************************
;********  INITIATIE  SUBROUTINE  ***********************************
;********************************************************************
	
INIT	

		;****************************************************
		;*******  INIT REGISTERS ****************************
		;****************************************************

		BCF STATUS,IRP		;voor indirect adresseren in bank 00

		BSF STATUS,RP0		;Bankselect OSCCON register
		BSF OSCCON,4		; van 4 naar 8MHz interne Oscilator
		BSF OSCCON,SCS
		BCF STATUS,RP0

		movlw b'00000001'
		movwf FLAG		;bits in FLAG register initieren  ??????
		

		;****************************************************
		;*******  INIT I/O POORTEN  *************************
		;****************************************************

		banksel ANSEL
		movlw b'10000000'
		movwf ANSEL		;ANS7 enable als analoog in (PORTE,2)
		CLRF ANSELH		;Geen analoge ingangen hoog
		
		banksel	TRISA		;0 is uitgang, 1 is ingang
		movlw b'11111111'
		movwf TRISA
		
		movlw b'00000001'		
		movwf TRISB		

		movlw b'00001111'		
		movwf TRISC	

		movlw b'11000011'		
		movwf TRISD	

		movlw b'11111101'
		movwf TRISE		

		;****************************************************
		;*******  INIT TIMER 0 ******************************
		;****************************************************

		banksel OPTION_REG
		movf OPTION_REG,w		;voor opwekking 20ms (50Hz)
		andlw b'11000000'	
		iorlw b'00000111'	
		       ;--0-----                 TOCS =0 (timer0 in timer mode)
		       ;----0---                 PSA=0 (prescaler voor timer0)
		       ;-----111                 prescaler=256
		movwf OPTION_REG 


		;****************************************************
		;*******  INIT TIMER 2 ******************************
		;****************************************************
	
		banksel T2CON
		movlw b'00100001'	;gebruikt voor opwekking servopulsen
		       ;------01         prescaler=4   =2µs	
		       ;-0100---         postscaler=1/5 =2x5=10µs per stap 10*256 stappen=2,56ms max pulsduur
		movwf T2CON


		;****************************************************
		;*******  INIT A/D CONVERTOR ************************
		;****************************************************

		banksel ADCON0
		movlw b'10011101'
		       ;10------	Fosc/32
		       ;--0111--	AN7 (portE,2)
		       ;-------1	ADC enalble
		movwf ADCON1

		;****************************************************
		;*******  INIT INTERRUPT ****************************
		;****************************************************
		
		banksel INTCON
		movlw b'11100000'
		movwf INTCON
		banksel PIE1
		movlw b'00000010'
		       ;------1-   	enable Timer 2 interrupt
		movwf PIE1

		

		;****************************************************
		;* Opvragen MIN, MAX, en VERTRAGING SERVO uit EEPROM*
		;****************************************************

		movlw 0x30		;startpositie HULPREG (aantal in te lezen data uit EEPROM)
		movwf HULPREG
		movlw 0x20		;startpositie  FSR indirect register (adres in RAM van var MINSERVO en in EEPROM)
		movwf FSR
		
I_LOOP
		banksel EEADR		;BANKSEL EEADR
		MOVF FSR,w  
		MOVWF EEADR 		;Data Memory
		
		banksel EECON1		;BANKSEL EECON1 ;
		BCF EECON1, EEPGD 	;Point to DATA memory
		BSF EECON1, RD 		;EE Read
		banksel EEDAT		;BANKSEL EEDAT ;
		MOVF EEDAT, W 		;EEdata naar POSserv,x
		movwf INDF		;
			
		incf FSR,1 		;verhoog adres 
		decf HULPREG,f		;verlaag HULPREG
		btfss STATUS,Z 		;als Carry bit <> 1 dan voer subr terug uit 
		goto I_LOOP
		banksel 0	


		;****************************************************
		;*******  INIT VARIABELEN ***************************
		;****************************************************
		
		INITPULS SCHAKELAAR1,PULS+0,MINSERVO+0,MAXSERVO+0,TIMSERVO+0,TELVERTR+0, SERVO1
		INITPULS SCHAKELAAR2,PULS+1,MINSERVO+1,MAXSERVO+1,TIMSERVO+1,TELVERTR+1, SERVO2
		INITPULS SCHAKELAAR3,PULS+2,MINSERVO+2,MAXSERVO+2,TIMSERVO+2,TELVERTR+2, SERVO3
		INITPULS SCHAKELAAR4,PULS+3,MINSERVO+3,MAXSERVO+3,TIMSERVO+3,TELVERTR+3, SERVO4
		INITPULS SCHAKELAAR5,PULS+4,MINSERVO+4,MAXSERVO+4,TIMSERVO+4,TELVERTR+4, SERVO5
		INITPULS SCHAKELAAR6,PULS+5,MINSERVO+5,MAXSERVO+5,TIMSERVO+5,TELVERTR+5, SERVO6
		INITPULS SCHAKELAAR7,PULS+6,MINSERVO+6,MAXSERVO+6,TIMSERVO+6,TELVERTR+6, SERVO7
		INITPULS SCHAKELAAR8,PULS+7,MINSERVO+7,MAXSERVO+7,TIMSERVO+7,TELVERTR+7, SERVO8
		INITPULS SCHAKELAAR9,PULS+8,MINSERVO+8,MAXSERVO+8,TIMSERVO+8,TELVERTR+8, SERVO9
		INITPULS SCHAKELAAR10,PULS+9,MINSERVO+9,MAXSERVO+9,TIMSERVO+9,TELVERTR+9, SERVO10
		INITPULS SCHAKELAAR11,PULS+.10,MINSERVO+.10,MAXSERVO+.10,TIMSERVO+.10,TELVERTR+.10, SERVO11
		INITPULS SCHAKELAAR12,PULS+.11,MINSERVO+.11,MAXSERVO+.11,TIMSERVO+.11,TELVERTR+.11, SERVO12
		INITPULS SCHAKELAAR13,PULS+.12,MINSERVO+.12,MAXSERVO+.12,TIMSERVO+.12,TELVERTR+.12, SERVO13
		INITPULS SCHAKELAAR14,PULS+.13,MINSERVO+.13,MAXSERVO+.13,TIMSERVO+.13,TELVERTR+.13, SERVO14
		INITPULS SCHAKELAAR15,PULS+.14,MINSERVO+.14,MAXSERVO+.14,TIMSERVO+.14,TELVERTR+.14, SERVO15
		INITPULS SCHAKELAAR16,PULS+.15,MINSERVO+.15,MAXSERVO+.15,TIMSERVO+.15,TELVERTR+.15, SERVO16
			
		RETURN 				;EINDE INITIATIE


;********************************************************************
;********  ADC VERWERKING  ******************************************
;********************************************************************

ADC_VERWERKING
		MOVF ADRESH,w		;ADC register ADRESH/8+1 --> VERTRAGING
		MOVWF VERTRAGING
		RRF VERTRAGING,F
		RRF VERTRAGING,F
		RRF VERTRAGING,W
		ANDLW b'00011111'
		MOVWF VERTRAGING
		INCF VERTRAGING,f

		MOVF ADRESH,w		;ADC register --> SERVOTIJD gelimiteerd tussen MINPULS en MAXPULS
		MOVWF SERVOTIJD
		SUBLW MINPULS
		BTFSS STATUS,C
		GOTO ADC_SPR1
		MOVLW MINPULS
		MOVWF SERVOTIJD
		GOTO ADC_END
ADC_SPR1	MOVF ADRESH,w
		SUBLW MAXPULS
		BTFSC STATUS,C
		GOTO ADC_END
		MOVLW MAXPULS
		MOVWF SERVOTIJD

ADC_END		BSF ADCON0,1		;herstart ADC conversie
		RETURN


;********************************************************************
;********  MAIN PROGRAM  ********************************************
;********************************************************************

		ORG 0x0800

MAIN
		pagesel INIT
		CALL INIT
		pagesel $			

LUS		
		pagesel ADC_VERWERKING
		CALL ADC_VERWERKING
		pagesel $
	
		SERVO MINSERVO+0, MAXSERVO+0, TIMSERVO+0, PULS+0, TELVERTR+0, SCHFF1,0,SCHAKELAAR1, SERVO1
		SERVO MINSERVO+1, MAXSERVO+1, TIMSERVO+1, PULS+1, TELVERTR+1, SCHFF1,1,SCHAKELAAR2, SERVO2
		SERVO MINSERVO+2, MAXSERVO+2, TIMSERVO+2, PULS+2, TELVERTR+2, SCHFF1,2,SCHAKELAAR3, SERVO3
		SERVO MINSERVO+3, MAXSERVO+3, TIMSERVO+3, PULS+3, TELVERTR+3, SCHFF1,3,SCHAKELAAR4, SERVO4
		SERVO MINSERVO+4, MAXSERVO+4, TIMSERVO+4, PULS+4, TELVERTR+4, SCHFF1,4,SCHAKELAAR5, SERVO5
		SERVO MINSERVO+5, MAXSERVO+5, TIMSERVO+5, PULS+5, TELVERTR+5, SCHFF1,5,SCHAKELAAR6, SERVO6
		SERVO MINSERVO+6, MAXSERVO+6, TIMSERVO+6, PULS+6, TELVERTR+6, SCHFF1,6,SCHAKELAAR7, SERVO7
		SERVO MINSERVO+7, MAXSERVO+7, TIMSERVO+7, PULS+7, TELVERTR+7, SCHFF1,7,SCHAKELAAR8, SERVO8

		pagesel VERVOLG	
		GOTO VERVOLG

		ORG 0x1000
VERVOLG	
		SERVO MINSERVO+8, MAXSERVO+8, TIMSERVO+8, PULS+8, TELVERTR+8, SCHFF2,0,SCHAKELAAR9, SERVO9
		SERVO MINSERVO+9, MAXSERVO+9, TIMSERVO+9, PULS+9, TELVERTR+9, SCHFF2,1,SCHAKELAAR10,SERVO10
		SERVO MINSERVO+.10,MAXSERVO+.10,TIMSERVO+.10,PULS+.10,TELVERTR+.10,SCHFF2,2,SCHAKELAAR11,SERVO11
		SERVO MINSERVO+.11,MAXSERVO+.11,TIMSERVO+.11,PULS+.11,TELVERTR+.11,SCHFF2,3,SCHAKELAAR12,SERVO12
		SERVO MINSERVO+.12,MAXSERVO+.12,TIMSERVO+.12,PULS+.12,TELVERTR+.12,SCHFF2,4,SCHAKELAAR13,SERVO13
		SERVO MINSERVO+.13,MAXSERVO+.13,TIMSERVO+.13,PULS+.13,TELVERTR+.13,SCHFF2,5,SCHAKELAAR14,SERVO14
		SERVO MINSERVO+.14,MAXSERVO+.14,TIMSERVO+.14,PULS+.14,TELVERTR+.14,SCHFF2,6,SCHAKELAAR15,SERVO15
		SERVO MINSERVO+.15,MAXSERVO+.15,TIMSERVO+.15,PULS+.15,TELVERTR+.15,SCHFF2,7,SCHAKELAAR16,SERVO16

WACHTLUS	btfss FLAG,FLAG20ms; wacht tot 20ms voorbij zijn
		goto WACHTLUS
		bcf FLAG,FLAG20ms
		MOVLW .98		;nieuwe waarde teller 0 (telt van 98-->256 = 20ms)
		MOVWF TMR0
		pagesel LUS
		goto LUS
  

;********************************************************************
;********  EEPROM WAARDES  ******************************************
;********************************************************************

		ORG 	0x2120		;start DATA EEPROM adres
					
		;bij eerste gebruik µC staan de volgende waardes in EEPROM: (waarde MAXSERVO komt overeen met 
		;middenstand nl. 1,5ms van eden servo. Altijd handig bij eerste gebruik µC met servo voor het
		;aanbrengen van de servoarm)

		DE 0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70,0x70 ;MINSERCO
		DE 0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80 ;MAXSERVO
		DE 0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0x02 ;TIMSERVO

		DE "Program om 16 servo's aan te sturen door middel van schakelaars, geschreven door Geert Giebens"


		END
;********************************************************************