;Servo PCM Controller
;version 03 Feb 2006
;copyright Bjoern Ch. Ludwar 


#include <p16F690.inc>
	__config (_INTRC_OSC_NOCLKOUT & _WDT_OFF & _PWRTE_OFF & _MCLRE_OFF & _CP_OFF &  _IESO_OFF & _FCMEN_OFF)

	cblock	0x20
POT1
POT2
COUNTER
HELP
POSI
DELAY							;for routine 'wait'
	endc
	
	
		org	0
START
   		call	INIT
		clrf	POSI				; POSI switches between positions
 		 
   		
MainLoop
	
		;get value of pot1
		bcf		ADCON0,2		;select AN0
		call	WAIT				;wait to stabilize

		bsf		ADCON0,GO		;start conversion from pot
		btfss	ADCON0,GO			;repeat until conversion is complete
		goto	$-1
		
		movf	ADRESH,w			;get pot value
		movwf	POT1				;and save
		rrf		POT1,1			;rotate right 3 times to reduce range
		rrf		POT1,1
		rrf		POT1,1
		movlw   B'00011111'
		andwf	POT1,1				;kill upper 3 bit (use only 32 values)
		movlw	0x56				;offset to center 1.5ms (0c5e for 16 steps)
		addwf	POT1,f				;add and leave where it is
		

		;get value of pot2
		bsf		ADCON0,2		;select AN1
		call	WAIT				;wait to stabilize

		bsf		ADCON0,GO		;start conversion from pot
		btfss	ADCON0,GO			;repeat until conversion is complete
		goto	$-1
		
		movf	ADRESH,w			;get pot value
		movwf	POT2				;and save
		rrf		POT2,1			;rotate right 3 times to reduce range
		rrf		POT2,1
		rrf		POT2,1
		movlw   B'00011111'
		andwf	POT2,1				;kill upper 3 bit (use only 32 values)
		movlw	0x56				;offset to center 1.5ms (0c5e for 16 steps)
		addwf	POT2,f				;add and leafe where it is
	

		;set servo to given position
		movf	POT2,w
		btfss	POSI,0				;if POSI jump over next instruction
		movf	POT1,w
		movwf	CCPR1L		


		;check keys
		btfss	PORTA,3				;check if key1 was pressed (skip next if bit A3 is 0)
		call 	KEY1

		btfss	PORTA,4				;check if key2 was pressed 
        call	KEY2

		goto	MainLoop			;if not start over
		



KEY1    call	WAIT					;debounce key - wait for 1ms
	    call	WAIT				;2ms
	    call	WAIT				;3ms
	    call	WAIT				;4ms
	    call	WAIT				;5ms
	    btfss	PORTA,3				;wait for key off again
		goto	KEY1
		
		movf	POT1,w				;move to other position
		btfss	POSI,0				;if POSI jump over next instruction
		movf	POT2,w
		movwf	CCPR1L

    	clrf	COUNTER					;wait approx. 256ms
LOOP   	call	WAIT
		decfsz	COUNTER,f
    	goto 	LOOP

		movf	POT2,w				;and back
		btfss	POSI,0				;if POSI jump over next instruction
		movf	POT1,w
		movwf	CCPR1L
	
    	
		return


KEY2	call	WAIT					;debounce key - wait for 1ms
	    call	WAIT				;2ms
	    call	WAIT				;3ms
	    call	WAIT				;4ms
	    call	WAIT				;5ms
	    btfss	PORTA,4				;wait for key off again
		goto	KEY2
		
		;toggle POSI
		movf	POSI,w
		xorlw	0xff
		movwf	POSI

		return

   		  		
	

INIT	clrf	PORTC					;switch all off
		call	PAGE1			
    	movlw	0xFF
		movwf	TRISA				;Make PortA all input
		clrf	TRISC				;Make PortC all output
		movlw	B'01100000'         		;A2D Clock Fosc/64
		movwf	ADCON1
    	
    	call	PAGE2	
		movlw	B'00100000'			;all Port A pins digital except RC5 (pot)
		movwf	ANSEL
	
		call 	PAGE0
		movlw	B'00000001'   
		movwf	ADCON0				;configure A2D for Channel 0 (RA0), left justified, turn on the AD
		call	WAIT				;wait for everything to settle
 	    	
    	clrf	CCP1CON					;force CCP1 pin PWM latch to default low 	
   		movlw	b'00001100'			;set CCP1 as one channel PWM 
       	movwf	CCP1CON
  
  		call	PAGE1
    	movlw   0Xff					;set highest PWM value
    	movwf   PR2

    	call	PAGE0
		movlw	B'00000110'			;set Timer 2 to no postscale, on, 16 prescale
		movwf	T2CON		
    	movlw	0x66
       	movwf	CCPR1L					;set PWM to 40% (1.5ms)
    
		return
		



PAGE0	bcf		STATUS,RP0			;address Register Page 0
		bcf		STATUS,RP1
		return
PAGE1	bsf		STATUS,RP0			;address Register Page 1
		bcf		STATUS,RP1
		return
PAGE2	bcf		STATUS,RP0			;address Register Page 2
		bsf		STATUS,RP1
		return
PAGE3	bsf		STATUS,RP0			;address Register Page 3
		bsf		STATUS,RP1
		return

WAIT	movlw	.71					; delay ~1000uS
		movwf	DELAY
		decfsz	DELAY,f				; this loop does 215 cycles
		goto	$-1		
		decfsz	DELAY,f				; This loop does 786 cycles
		goto	$-1
		return



		end