; DIY PIC Servo
;-------------------------------------------------------------------
; This program accepts input from a rotary encoder through bits ra.0 and ra.1,
; determines the direction of rotation, and increments or decrements a counter
; appropriately. It drives a stepping motor at a pulse width of 2msec, for a  
; stepping speed of 250/sec. The counter 'zero' value is 128, so that the 
; maximum number of steps counted is +/- 128 before steps are lost.
; Raising bit 5 of port b will result in the stepper rotating continuously
;------------------------------------------------------------------------------
; V2.0   18 Sept 2000
; Converted from original Parallax code to Microchip PASM in order to use 16C54C 
; (the aim here has been to produce exactly the same .obj code as the original)
;-----------------------------------------------------------------------------
;       Port A,  Bit 0 = Encoder input
;                    1 = ,,      ,,
;                    2 =  
;                    3 = 		
;
;       Port B,  Bit 0 = Stepper direction 
;                    1 = Stepper control 
;                    2 = 
;                    3 = 
;                    4 = 
;                    5 = flash input (rotate continuously)
;                    6 = 
;                    7 = 
;-------------------------------------------------------------------
 	LIST P=16C54C
     	include "P16c5x.inc"       		; standard PIC definition file
     	__CONFIG _CP_OFF & _WDT_OFF & _RC_OSC   ; set fuses                      

	ORG     H'1FF'             		; processor reset vector
	goto    Start

;   	Ram starts at H'07'

Temp            EQU  H'0C'
Counter         EQU  H'0D'
Old             EQU  H'0E'    
New             EQU  H'0F'    
Flag            EQU  H'1F'
Armed           EQU  H'1E'

;-------------------------------------------------------------------
		ORG 	H'00'

Start		movlw  	B'00100000'		; PortB, only B5 is input  
      	tris   	PORTB
		movlw  	B'11111111'		; all PortA is input
      	tris   	PORTA
      	
		movlw  	D'128'   		; init counter 
		movwf		Counter    	
		bcf		PORTB,1   		; make Output low

	      movf   	PORTA,W		; get encoder setting
      	movwf  	Old			; and save it
        	movlw  	B'00000011'		; mask out encoder bits
        	andwf  	Old,F

Loop		btfsc  	PORTB,5		; continuous rotation required ?
        	goto   	Test
        	call   	Check_encoder	; has encoder moved ?

Flashing    movlw  	D'128'		; has counter reached zero position ?
            subwf  	Counter,W
		btfsc  	STATUS,Z
            goto   	Loop			; if so, do nothing

		btfsc  	Flag,0      	; is timer running ? 
            goto   	Timeout      
		movlw  	B'00000111'		; no, start 2 msec timer
		option
		movlw  	D'248'
            movwf  	TMR0
		bsf    	Flag,0		; set Flag

Timeout	movf   	TMR0,W		; 2 msec timeout yet?
		btfsc  	STATUS,Z
            goto   	Drive			; yes, drive it 
            goto   	Loop 			; no, keep waiting

Drive		bcf    	Flag,0		; clear timer flag
		btfsc  	PORTB,1		; flip output pin
            goto   	Lower
		bsf    	PORTB,1		; Pos output pulse
		bcf    	Armed,0		; no increment on rising edge
		goto   	Cont
Lower		bcf    	PORTB,1
            bsf    	Armed,0		; pulse completed, allow increment

Cont		movlw  	D'127'			
            addwf  	Counter,W
		btfsc  	STATUS,C		
            goto   	Down

Up		bcf    	PORTB,0		; setup for Anticlock rotation
		btfsc  	Armed,0		; if rising edge, no increment
		incf   	Counter,F
            goto   	Try_done

Down		bsf    	PORTB,0		; setup for Clockwise rotation
		btfsc  	Armed,0		; if rising edge, no increment
            decf   	Counter,F		; output pulse finished

Try_done    movlw  	D'128'		; is Counter back at zero ?
            subwf  	Counter,W
            btfss  	STATUS,Z
            goto   	Loop			; yes
		bcf    	Flag,0		; no, enable timer restart
            goto   	Loop

;-------------------------------------------------------------------
; check to see if Encoder has moved
;   if so, adjust the counter up or down

Check_encoder   movf   	PORTA,W		; get latest state of input bits
	      movwf  	New
            movlw   	B'00000011'		; mask off all but the encoder bits
            andwf   	New,F

		movf   	New,W			; has encoder moved ?
		movwf  	Temp
		movf   	Old,W			; is new = old ?
		xorwf  	Temp,F
		btfsc  	STATUS,Z
		goto		_return		; if not, return without changing counter
   
		bcf    	STATUS,C		; clear carry in preparation for rotate-left
		rlf    	Old,F             ; move old to the left to align old.0 with new.1 
		movf   	New,W
		xorwf  	Old,F
		btfsc  	Old,1			; if the XOR result is 0, decrecemt counter
		goto   	CountUp		; otherwise increment

CountDn	decf   	Counter,F
		btfss		FSR,7			; parallax skip
CountUp	incf   	Counter,F
C00		movf   	New,W
		movwf  	Old
_return	retlw  	0

;-------------------------------------------------------------------

Test		movlw  	D'129'		; make it think it has moved
		movwf  	Counter		; to give continuous rotation
		goto   	Flashing

;-------------------------------------------------------------------
	END