TITLE "Servo Controller"
	     LIST P = 16C55
;            DATE 01SEP94
;            AUTHOR RICK FARMER
;            FILE NAME servo.ASM
;              Clock = 4MHz -> 1 instruction takes 1us
PIC55   equ     1FFH
STATUS  equ     3h              ; F3 Reg is STATUS Reg.
PortA   equ     5h
PortB   equ     6h              ; I/O Port Assignments
PortC   equ     7h
PULSE   equ    0Bh              ; pulse time high
DELAY   equ    0Ch              ; 1 ms count
DOWN    equ    0Dh              ; # ms /4 of low time
CAP     equ    0Eh              ; time to charge cap
LOWPUL  equ    0FH              ; pulse time high for duty cycle
Z       equ     2h
C       equ     0h
;********************************************************************

START   MOVLW  B'000111'        ;Select RTCC, internal clock source
				;  & prescale value = 256
	OPTION                  ;Actually load OPTION reg.
	movlw  00h              ;setup port A as output
	tris   PortA 
	movwf  PortA            ;clear port A
	movlw  0FFh             ;setup ports B & C as input
	tris   PortB
	tris   PortC  

BEGIN   MOVLW  0FFh             ;load 1ms. counter
	MOVWF  DELAY            ;get it into reg
	BSF    PortA,0          ;start pulse output

HIMS    NOP                     ;force loop into 4 cycle duty (even #)
	NOP
	DECFSZ DELAY            ;loop for 1st 1ms. of duty cycle 
	 GOTO  HIMS

HIGH    NOP                     ;force loop into 4 cycle duty (even #)
	NOP
	NOP
	DECFSZ PULSE            ;loop for high part of duty cycle 
	 GOTO  HIGH             ;while pulse > 0  
	BCF    PortA,0          ;end pulse output
	
HIPUL   NOP                     ;force loop into 4 cycle duty (even #)
	NOP                     ;duty cycle compensation
	INCFSZ LOWPUL           ;loop for high part of duty cycle 
	 GOTO  HIPUL            ;while pulse > 0  

	MOVLW  03h              ;load # ms * 4 counter
	MOVWF  DOWN             ;get it into reg
	MOVLW  0FAh             ;load 1ms. counter
	MOVWF  DELAY            ;get it into reg
	
LOW     NOP                     ;force loop into 4 cycle duty (even #)
	NOP  
	  INLOOP  NOP           ;force loop into 4 cycle duty (even #)
		  NOP 
		   DECFSZ DELAY ;loop for low part of duty cycle 
		  GOTO INLOOP
	DECFSZ DOWN             ;loop for low part of duty cycle 
	 GOTO  LOW

	movlw  0ffh             ;make rb1 an input
	tris   PortB            ;to let the cap charge

CHARGE  INCFSZ   CAP,1          ;start timing loop
	 GOTO     NEXT          ;check for overflow
	DECF     CAP,1          ;make it FFh
	GOTO     DONE
NEXT    NOP                     ;timing nop
	NOP
	BTFSS  PortB,1          ;check input pin RB1
	 GOTO   CHARGE          ;not charged yet

DONE    MOVF   CAP,0            ;but time in W
	MOVWF  PULSE            ;save time in
	MOVWF  LOWPUL

DUTY    NOP                     ;force this part to a constant 2ms
	NOP
	NOP
	NOP
	NOP
	INCFSZ CAP,1            ;loop unit roll over for constant duty cycle
	 GOTO   DUTY            

	movlw  0fdh             ;make rb1 an output
	tris   PortB            ;drive it low to discharge timing cap
	bcf    PortB,1          ;for a constant time

	GOTO   BEGIN            ;restart loop
	ORG    PIC55
	GOTO   START
	END