; Andy Meng
; this program controls 2 R/C type servos with the control lines
; on pins RB1 and RB2
	errorlevel -302		; turn off annoying bank bits notification

	__CONFIG _BODEN_OFF & _CP_OFF & _HS_OSC & _PWRTE_OFF & _WDT_OFF & _LVP_OFF
	include "p16f872.inc"

	org 0
	goto	Init

; pin equates
Servo1Pin equ	2
Servo2Pin equ	1

; memory locations
Servo1Pos equ	0x20
Servo2Pos equ	0x21
ServoDelay equ	0x22
d1	  equ	0x23
d2	  equ	0x24
servocount equ  0x25

	org 04

ISR:
	bcf	INTCON,GIE	; disable global interrupts
	bcf	PIR1,TMR1IF
	clrf	T1CON		; turn off timer
	call	Servo1		; set the first servo
	call	Servo2		; set the second servo
	clrf	TMR1H		; clear timer high byte
	clrf	TMR1L		; clear timer low byte
	movlw	0xBD		; put 0xBD in w (BD97 = 17ms delay)
	movwf	TMR1H		; put it in the high timer byte
	movlw	0x97		; put 0x97 in w
	movwf	TMR1L		; put it in the low timer byte
	movlw	0x29		; turn on the timer again
	movwf	T1CON
	incf	Servo2Pos,f
	decf	Servo1Pos,f
	retfie			; return from interrupt

	org 20

Init:
	movlw	0xC0	; ints on, peripheral ints on
	movwf	INTCON
	movlw	0x29		; turn on timer, prescaler, internal clock
	movwf	T1CON
	bsf	STATUS,RP0
	clrf	PIE1
	bsf	PIE1,TMR1IE	
	clrf	TRISC
	bcf	STATUS,RP0
	clrf	PORTC
	movlw	0xFF		; 0x00 is actually 0xFF (decfsz substracts one)
	movwf	Servo1Pos
	movlw	0x01
	movwf	Servo2Pos
wait:
	; the rest of the code would go here
	goto	wait	

Servo1:				; outputs pulse for servo1
	bsf	PORTC,Servo1Pin	; set servo1 pin high
	call	Delay1ms	; delay 1 ms, then add on 0 to 1 ms delay)
	movf	Servo1Pos,w	; put servo1 position in w
	movwf	ServoDelay	; load the servo1 position into ServoDelay
Servo1Delay:			; Delay loop (0 to 1020us)
	call	ServoPulseDelay	; this routine takes about 5us to execute
	decfsz	ServoDelay,f	; decrement ServoDelay
	goto	Servo1Delay	; if it isn't done, go to the top of the loop
	bcf	PORTC,Servo1Pin ; end output pin
	return			; if it is done, return

Servo2:				; outputs pulse for servo2
	bsf	PORTC,Servo2Pin
	call	Delay1ms
	movf	Servo2Pos,w
	movwf	ServoDelay
Servo2Delay:
	call	ServoPulseDelay
	decfsz	ServoDelay,f
	goto	Servo2Delay
	bcf	PORTC,Servo2Pin
	return

Delay1ms:
		
	movlw	0x08		; was 0x23 (for 1ms) got 08 by trial and error (actually .5ms)
	movwf	d1
Delay_00
	movlw	0x38
	movwf	d2
Delay_01
	decfsz	d2, f
	goto	Delay_01
	decfsz	d1, f
	goto	Delay_00

			;103 cycles
	movlw	0x22
	movwf	d1
Delay_10
	decfsz	d1, f
	goto	Delay_10
	return

ServoPulseDelay:
	movlw	0x0D		; obtained by trial and error
	movwf	servocount
	decfsz	servocount,f
	goto	$ - 1
	return	

	end