; CPU configuration
	processor 16f84
	include	  <p16f84.inc>
	__config  _HS_OSC & _WDT_OFF & _PWRTE_ON

; variables

; delay code is 4 cycles long, at 4Mz one instruction is 1us, so we can
; execute 250 4 cycle delay loops per ms

; the first time we wait for just under 1ms (load the counter with 248) after 
; activating the servo. then we use the position value to set the delay length
; (in 4us increments) when the delay finishes, we activate the servo. the 
; result is a 1-2 ms pulse corresponding to the position variable

; the servo needs updating every 20ms or so

counter		EQU	H'0c'	; servo delay counter register
position	EQU	H'0d'	; servo position register
outpin		EQU	H'01'	; servo i/o pin
outpin2 	EQU     H'02'   
innerdelay	EQU	H'10'	; delay for random positioning
temp		EQU	H'11'
ping		EQU	H'12'

main:
		movlw   B'00000000'
	        tris    PORTA           ; init port A as output
		movlw   B'11111111'
	        tris    PORTB           ; init port B as input

		movlw	D'00'		; init variables
		movwf	counter
		movwf	position

start:
		bsf	PORTA,outpin	; activate servo output
		bcf	PORTA,outpin2
		movf	position,0	; load w with servo position
		movwf	counter		; and store in Counter for next delay
		call	delay		; keep servo active for position delay

		; calculate how long the second delay needs to be
		movlw	D'255'
		movwf	temp
		movf	position,0
		subwf	temp,0
		movwf	counter

		bcf	PORTA,outpin	; deactivate servo output
		bsf	PORTA,outpin2
		call	delay

		btfss	ping,0		; if ping is set, incr
		goto 	decpos          ; otherwise decr
		incf	position,1	; inc!
		goto 	endpos		; skip the decr
decpos:
		decf	position,1 	; decr!
endpos:
		movlw	position	; load position
		btfss	STATUS,Z	; .. and test for zero
		goto	start
		comf	ping,1
		goto	start

delay:
		movlw	D'55'
		movwf   innerdelay
inner:
		nop			; pad loop
		decfsz  innerdelay,f    ; inner loop
		goto	inner

		decfsz	counter,f	; counter=counter-1
		goto 	delay
		return
end