; 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
position2       EQU     H'11'
servopin	EQU	H'01'	; servo i/o pin
servopin2       EQU     H'02'
inputpin	EQU	H'01'	; mode switch i/o pin
longcounter	EQU	H'0e'	; long delay counter
random		EQU	H'0f'	; random state
randomcounter	EQU	H'10'	; delay for random positioning

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
		movwf	longcounter
		movwf	randomcounter
		movlw	D'79'		; random seed
		movwf	random

servo:
               
		movlw	D'248'		; load counter with cycles for 
		movwf	counter		; 1 ms servo delay
		bsf	PORTA,servopin	; activate servo output
		call	delay		; wait for 1ms to pass
		movf	position,0	; load w with servo position
					; 00 and FF=extremes 128 = centre
		movwf	counter		; and store in Counter for next delay
		call	delay		; keep servo active for position delay
		bcf	PORTA,servopin	; deactivate servo output


		movlw	D'8'		; wait 10ms ish
		movwf	longcounter
		call	longdelay





		movlw	D'248'		; load counter with cycles for 
		movwf	counter		; 1 ms servo delay
		bsf	PORTA,servopin2	; activate servo output
		call	delay		; wait for 1ms to pass
		movf	position2,0	; load w with servo position
					; 00 and FF=extremes 128 = centre
		movwf	counter		; and store in Counter for next delay
		call	delay		; keep servo active for position delay
		bcf	PORTA,servopin2	; deactivate servo output



		movlw	D'8'
		movwf	longcounter
		call	longdelay

		incf    position2
                
		goto	randompos

randompos:
		decf	randomcounter,f
		btfss	STATUS,Z	; skip if it's zero
		goto	servo
		movlw	D'32'
		movwf	randomcounter
		call	rnd
		movf	random,w
		movwf	position	; load position with random pos
		goto	servo

longdelay:
		movlw	D'248'		; load counter with cycles for 
		movwf	counter		; 1 ms servo delay
		call	delay		; wait for 1ms to pass
		decfsz	longcounter,f	; counter=counter-1
		goto 	longdelay
		return

delay:
		nop			; pad loop
		decfsz	counter,f	; counter=counter-1
		goto 	delay
		return

rnd:	
		movlw	01DH
		clrc	
		rlf	random,f
		skpnc
		xorwf   random,f
		retlw   0
end