;****************************************************************************
;Dbt_servo_tst
;Rotates servo position in 45 degree steps from 0 degs to 180 degs.
;Servo PWM drive period, is generated with interrupt on overflow 
;(Timer 2), with pulse width in software. 
;A fixed number of pulses are emitted at each position, with ramp 
;generated between each one.
;TJW 26.5.05											Tested 30.8.05
;***************************************************************************
;Clock is 4MHz
;
	list 	p=16F873A
	#include p16f873A.inc
;
;Set Configuration Word: crystal oscillator HS, WDT off, 
;     power-up timer on, code protect off, LV Program off.
	__CONFIG _HS_OSC & _WDT_OFF & _PWRTE_ON & _LVP_OFF
;
;Specify RAM
	cblock 20
delcntr1 	;used in delay5 & delayADC SRs
delcntr2 
temp	  	;a temp location, to be used only in consecutive instructions
int_cntr 	;counts incoming interrupts
pulse_cntr  ;counts how many pwm pulses sent to servo, before moving to next posn.
pw_cntr	    ;counter used to set width of pulse to servo
servo_dirn  ;little counter which determines direction servo points, used as look-up table pointer	
past_pulse  ;holds value of most recent pulse width, used with demand value to determine actual
	endc
;
	org 00
	goto begin
;
	org	04
	goto ISR
;Initialise
;set up SFRs in Bank 1
begin	bcf	status,rp1
	bsf	status,rp0 		;select memory bank 1
	movlw	B'00011011'	;set port A bits, bit 4 ip for opto
	movwf	trisa		
	movlw	B'11110001'
	movwf	trisb		;also port B bits, bit 3 is servo PWM op
	movlw	B'10000001' ;bit 0 ip for opto
	movwf	trisc		;and port C bits
	movlw	B'01000100' 
	movwf	adcon1		;select port A bits 0,1,3 for analog input
	movlw	B'11101000'
	movwf	option_reg	
	movlw 	D'250'			;set Timer 2 Overflow period
	movwf	pr2
;set up SFRs in Bank 0	
	bcf	status,rp0 		;select bank 0
	movlw	B'01111100'	;switch on Timer2, no prescale, /16 postscale
						  ;giving 4ms interrupt prd (with pr2=250)
	movwf	t2con
;
;Initialise counter values
	movlw	D'60'	;start with intermediate pulse width
	movwf	past_pulse
	movlw	D'5'	;preset interrupt counter
	movwf	int_cntr
	movlw	D'200'
	movwf	pulse_cntr	;preload pulse counter
;Switch all outputs off
	movlw 00
	movwf porta
	movwf portb
	movwf portc
;diagnostic, flash leds
	bsf portc,6
	bsf portc,5
	call delay500
	bcf portc,6
	bcf portc,5
	call delay500
;Enable interrupts
	bcf	pir1,tmr2if		;clear pending interrupts
	bsf	status,rp0 		;select memory bank 1
	bsf	pie1,tmr2ie		;enable Timer 2 interrupt
	bcf	status,rp0 		;select memory bank 0
	bsf	intcon,peie		;enable peripheral interrupts
	bsf	intcon,gie		
wait goto wait	;let ISR do the work
;
;******************************************************************
;ISR is here. Interrupts occur every 4ms. Count 5, and on 5th
;emit pulse to Servo. Pulse length will depend on servo_dirn setting
;******************************************************************
ISR	decfsz	int_cntr	;decrement interrupt counter. Action occurs 
				   ;only if it is 0.
	goto	intend
	decfsz	pulse_cntr	;here if a pulse to be output, test if pulse 
				   ;width is to change
	goto	ISR1
	incf   servo_dirn,1 ;here if pulse width changing, point to new
		   ;duration
	movlw	D'200'		;reload pulse counter, 200 pulses will be emitted
	movwf	pulse_cntr	   ;at new length
;now determine pulse width, calculating ramp value if needed
ISR1 movf servo_dirn,0 ;get demand pulse duration from Table
	andlw	07		;use only 3 lsbs of servo_dirn
	call 	int_table
	movwf	pw_cntr	;this is demand value, may need to ramp towards it
;now determine ramp value, if needed 
	subwf	past_pulse,0	;compare demand value with most recent pulse
	btfsc	status,z
	goto	pulse_op	;if equal, go direct to pulse
	btfsc	status,c	;if carry clear, demand>past
	goto	$+3
	incf	past_pulse,1 ;here if demand>past, hence ramping up
	goto	$+2
	decf	past_pulse,1 ;here if demand<past, hence ramping down
	movf	past_pulse,0 ;and save new value for next time round
	movwf	pw_cntr	
;now send pulse
pulse_op bsf	portb,3	;set pulse high
pw_loop call delay20u
	decfsz	pw_cntr
	goto	pw_loop
	bcf		portb,3		;clear pulse
	movlw	D'5'	;reload interrupt counter
	movwf	int_cntr
intend bcf pir1,tmr2if ;clear interrupt flag
	retfie		
;Table for servo positions
int_table addwf pcl
	retlw D'20'		;400us delay, 0 degrees
	retlw D'40'		;800us delay, 45 degrees
	retlw D'60'		;1200us delay, 90 degrees
	retlw D'80'		;1600us delay, 135 degrees
	retlw D'100'	;2000us delay, 180 degrees
	retlw D'80'		;1600us delay, 135 degrees
	retlw D'60'		;1200us delay, 90 degrees
	retlw D'40'		;800us delay, 45 degrees
;
;**********************************************************
;SUBROUTINES
;**********************************************************
;introduces delay of 20us 
delay20u  movlw 5	;5 cycles called, each taking 3us, 
		;plus call, return (2 ea), and 2 move insts
;			less one cycle lost when last goto is hopped
        movwf  	delcntr1
dela    decfsz 	delcntr1,1 ;3 inst cycles in this loop, ie 3us
        goto 	dela
        return
;

;introduces delay of 1ms approx
delay1  movlw 	D'250'	;250 cycles called, 
;			each taking 4us
        movwf  	delcntr1
del1    nop	;4 inst cycles in this loop, ie 4us
        decfsz 	delcntr1,1
        goto 	del1
        return
;
;introduces delay of 500ms approx, thru 500 calls to delay1
delay500  movlw D'250'
        movwf  	delcntr2
del5    call 	delay1
		call 	delay1
        decfsz  delcntr2,1
        goto 	del5
        return  
;
	end
