;*****************************************************************************
;   reverser.asm             Servo Reverser
;*****************************************************************************
;                Bruce Abbott   bhabbott@paradise.net.nz
;
;                        for Microchip 12F615, 12F675 or 12F683
;
;=============================================================================
;                             Summary of Changes
;
; 2008/06/03 V0.0 - Created
; 2010/04/30      - changed version string to suit PICkit2 
; 2010/05/16 V0.1 - changed I/O pin assignments for better board layout
;                 - added option jumper
; -----------------------------------------------------------------------------

#DEFINE version  " 0 . 1"

;#DEFINE __12F615	; MPLAB users should use menu <Configure/Select Device>
;#DEFINE __12F675
;#DEFINE __12F683

	ifdef	  __12F615
        PROCESSOR PIC12F615
        INCLUDE   <P12F615.inc>
#DEFINE	RAMSTART  0x40
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_IOSCFS_4MHZ&_INTRC_OSC_NOCLKOUT
	endif

	ifdef	  __12F675
        PROCESSOR PIC12F675
        INCLUDE   <P12F675.inc>
#DEFINE	RAMSTART  0x20
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_BODEN_ON&_INTRC_OSC_NOCLKOUT
	endif

	ifdef	  __12F683
        PROCESSOR PIC12F683
        INCLUDE   <P12F683.inc>
#DEFINE	RAMSTART  0x20
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_BOD_ON&_INTRC_OSC_NOCLKOUT
	endif

	errorlevel 0,-305,-302
        radix     dec	; decimal numbers

; options

;#DEFINE	TRIM1	1	; trimpot affects output 1
;#DEFINE	TRIM2	1	; trimpot affects output 2
;#DEFINE	STATUSLED 1	; status LED active

; Bit definitions for the GPIO register and the TRIS register

#DEFINE JP1      0    ; pin 7  (GP0) option jumper
#DEFINE	SERVO1	 1    ; pin 6  (GP1) 1st servo output (1~2mS pulse) normal
#DEFINE SERVO2	 2    ; pin 5  (GP2) 2nd servo output (1~2mS pulse) inverted
#DEFINE PULSE	 3    ; pin 4  (GP3) servo input (1~2mS pulse)
#DEFINE	POT	 4    ; pin 3  (GP4) trim pot, input to A/D
#DEFINE LED	 5    ; pin 2  (GP5) status indicator. Active Low (LED+resistor to Vcc)


	ifdef	STATUSLED
#DEFINE TrisBits (1<<PULSE)|(1<<POT)|(1<<JP1)	; define which pins will be inputs
	else
#DEFINE TrisBits (1<<PULSE)|(1<<POT)|(1<<JP1)|(1<<LED)	
	endif

; -------------------- OPTIONS ------------------------
;   weak pullups enabled
;   Timer 0 source internal
;   Prescaler to Timer 0, divide by 256.

#DEFINE OptionBits B'00000111'

;========================================================================
;                 16 bit macros (little-endian)
;

addi16	MACRO val, dest ; dest=dest+val
	movlw val % 0x0100
	addwf dest,F
	movlw val / 0x0100
	movwf AARGB0
	skpnc
	incfsz AARGB0,W
	addwf dest+1,F
	ENDM

subi16	MACRO val, dest ;dest=dest-val
	movlw val / 0x0100
	movwf AARGB0
	movlw val % 0x0100
	subwf dest,F
	movf AARGB0,W
	skpc
	incfsz AARGB0,W
	subwf dest+1,F	; If C then dest >= 0
	ENDM

add16	MACRO al, bl ;b=b+a
	movf al,W
	addwf bl,F
	movf al+1,W
	skpnc
	incfsz al+1,W
	addwf bl+1,F
	ENDM

sub16	MACRO al,bl  ;b=b-a
	movf al,W
	subwf bl,F
	movf al+1,W
	skpc
	incfsz al+1,W
	subwf bl+1,F	; If C then a1 >= a2
	ENDM

rr16	MACRO al ;a=a/2
	rrf al+1
	rrf al
	ENDM

rl16	MACRO al ;a=a*2
	rlf al
	rlf al+1
	ENDM

inc16	MACRO	al ;a=a+1
	incfsz	al
	decf	al+1
	incf	al+1
	ENDM

dec16	MACRO	al ;a=a-1
	tstf	al
	skpnz
	decf	al+1
	decf	al
	ENDM

com16	MACRO	al ; a = NOT a
	comf	al
	comf	al+1
	ENDM

movi16	MACRO val, dest ;dest=val
	movlw val % 0x0100
	movwf dest
	movlw val / 0x0100
	movwf dest+1
	ENDM

mov16	MACRO src, dest ;dest=src
	movf  src,w
	movwf dest
	movf  src+1,w
	movwf dest+1
	ENDM

;==========================================================================
; Macro for generating short time delays
;
NO_OP           MACRO   count
NO_OP_COUNT     SET     count
                WHILE   NO_OP_COUNT>1
		goto	$+1		; 2 clocks
NO_OP_COUNT     SET     NO_OP_COUNT-2
                ENDW
		IF	NO_OP_COUNT
		nop			; 1 clock
		ENDIF
                ENDM

;===========================================================================
; Macros to create offsets for variables in RAM
;
ByteAddr	SET	RAMSTART		; user RAM starts here

BYTE            MACRO	ByteName
ByteName        EQU	ByteAddr
ByteAddr	SET	ByteAddr+1
                ENDM

WORD		MACRO	WordName
WordName	EQU	ByteAddr
ByteAddr	SET	ByteAddr+2
		ENDM

; ==========================================================================
;                 RAM Variable Definitions
;
        BYTE	Flags		; various boolean flags

	WORD	PulseCount	; measured input pulse width 0.9~2.1mS

	WORD	LastCount	; previous pulse width

        WORD	ServoCount	; servo output pulse width

	WORD	Servo1		; 1st servo output pulse width

	WORD	Servo2		; 2nd servo output pulse width

	WORD	Trim		; servo trim value

	WORD	PotVolts	; Potentiometer Voltage 

	BYTE	Glitches	; number of consecutive bad servo pulses

	BYTE	Temp1
	BYTE	Temp2
	BYTE	t1
	WORD	aa		; 16 bit working register
	WORD	AARGB0		; 16 bit working register

	BYTE	w_temp		; interrupt context storage
	BYTE	FSR_temp
	BYTE	status_temp




; flag values
;
#DEFINE WATCH		0	; Watchdog timed out
#DEFINE	GOT_FS		1	; got failsafe value
#DEFINE	PULSEHI		2	; servo pulse was high
#DEFINE	GOTPULSE	3	; got servo pulse input
#DEFINE	BADPULSE	4	; bad servo pulse input

;****************************************************************************
;                                Code
;
	ORG	0
	goto	ColdStart	; jump to power-on startup

	ORG	4
	goto	isr		; jump to interrupt service routine

;-------------------------- version string ----------------------------------

	org	8
	dw	" R V S R"
	ifdef	__12F615
	dw	" 6 1 5  "
	endif
	ifdef	__12F675
	dw	" 6 7 5  "
	endif
	ifdef	__12F683
	dw	" 6 8 3  "
	endif
	dw	" V"
	dw	version

;============================================================================

isr:		movwf   w_temp
		movf	STATUS,w
		movwf	status_temp     ; save registers
		movf	FSR,w
		movwf	FSR_temp
		bcf 	STATUS,RP0	; page zero

		btfsc	PIR1,TMR1IF	; Timer 1 overflow ?
		goto	long_pulse

		btfsc	GPIO,PULSE	; leading edge of pulse detected ?
		goto	pulse_high	; yes

pulse_low:	btfss	Flags,PULSEHI	; was pulse high ?
		goto	pulse_done	; no, exit
		bsf	Flags,GOTPULSE
		bcf	INTCON,GPIF	; clear pin change interrupt
		nop			
		bcf	T1CON,TMR1ON	; Stop timer1
		movf	TMR1L,w
		movwf	PulseCount		
		movf	TMR1H,w		; get timer1 value
		movwf	PulseCount+1		
		goto	exit_isn	; done

pulse_high:	bcf	T1CON,TMR1ON	; Stop timer1
		movlw	(0)%256
		movwf	TMR1L 		; load timer1 
		movlw	(0)/256
		movwf	TMR1H
		bsf	T1CON,TMR1ON	; start timer1
		bsf	Flags,PULSEHI	; now waitng for pulse low
pulse_done:	bcf	INTCON,GPIF	; clear pin change interrupt

; exit and enable interrputs

exit_isr:	movf    FSR_temp,w
		movwf	FSR
		movf    status_temp,w	; restore registers
		movwf	STATUS
		swapf   w_temp,f
		swapf   w_temp,w
		retfie

; Timer1 overflow

long_pulse:	bcf	T1CON,TMR1ON	; Stop timer1
		bcf	PIR1,TMR1IF	; clear timer1 interrupt
		bsf	Flags,BADPULSE	; bad pulse !		

; exit with interrupts disabled

exit_isn:	movf    FSR_temp,w
		movwf	FSR
		movf    status_temp,w	; restore registers
		movwf	STATUS
		swapf   w_temp,f
		swapf   w_temp,w
		return



;===========================================================================
;              Startup from Power On or Watchdog Timeout
;	
ColdStart:
	bcf	Flags,WATCH
	btfss	STATUS,NOT_TO		; copy Watchdog timeout flag
	bsf	Flags,WATCH

; 12F683: set internal oscillator frequency
	ifdef	__12F683
	bsf	STATUS,RP0
	movlw	b'01100001'		; 4MHz internal RC Oscillator
	movwf	OSCCON
	bcf	STATUS,RP0
	endif

; 12F675: get oscillator calibration value and use it to fine-tune clock frequency.
	ifdef	__12F675
	bsf	STATUS,RP0		; register bank 1 (12F629/75)
	call	0x3ff			; get OSCCAL value
	movwf	OSCCAL			; set oscillator calibration
        bcf	STATUS, RP0		; register bank 0
	endif

; set options

	bsf	STATUS,RP0		; register bank 1 
        movlw   OptionBits
        movwf	OPTION_REG
	bcf	STATUS,RP0		; register bank 0

;========================================================================
; initialise I/O registers

        clrf	GPIO			; all outputs off

	banksel	TRISIO			; register bank 1

        movlw	TrisBits
        movwf	TRISIO			; set I/O pin directions

	banksel	0			; register bank 0

	ifdef	__12F615
	movlw	b'00000000'
        movwf	CMCON0			; Comparator off
	movwf	CCP1CON			; CCP module off
	endif

	ifdef	__12F675
	movlw	b'00000111'
        movwf	CMCON			; Comparator off
	endif

	ifdef	__12F683
	movlw	b'00000111'
	movwf	CMCON0			; comparator off
	movlw	b'00000000'
	movwf	CCP1CON			; CCP module off	
	endif

; set up A/D convertor
	banksel	ANSEL
	movlw	b'00011000'	; clk/8, AN3 active
	movwf	ANSEL
	banksel	0
	movlw	b'00001101'	; left-justify, Vdd ref, input AN3, A/D on
	movwf	ADCON0

; CPU specific stuff done, now we can start the main program!

        goto	Main


;========================================================================
;                     Read Potentiometer Voltage
;
ReadPot:	
	movlw	64		; 64 x oversampling
	movwf	Temp1
	clrf	PotVolts	; no sample yet!  
	clrf	PotVolts+1
pot_read:
	bsf	ADCON0,GO	; start A/D conversion
pot_wait:
	btfsc	ADCON0,GO	; wait until A/D done
	goto	pot_wait
	movf	ADRESH,w	; get A/D value 0-255
	addwf	PotVolts
	skpnc			; accumulate readings  
	incf	PotVolts+1
	decfsz	Temp1		; next reading
	goto	pot_read
	movlw	6
	movwf	Temp1		; 2^6 = 64
pot_divide:
	clrc
	rr16	PotVolts	; divide by 64 
	decfsz	Temp1
	goto	pot_divide
	return

;**************************** SERVO OUT ********************************
;
; based on code by mmormota
; http://www.rcgroups.com/forums/showthread.php?t=624290
;
; 16 bit input expected in Servo1, Servo2
;
; Pulses start consecutively, with 1uS resolution.
; pulses end (almost) simultaneously, with 32uS resolution.
; 
;                       ________________ 
; Servo1   ___//////////                \     \     \     \_____
;               0~32uS                     32uS*(pulse/32)  
;                                  ________
; Servo2   _____________///////////        \     \     \     \_____
;                          0~32uS             32us*(pulse/32)
;
servo_out:
		subi16 83,Servo1	
		subi16 26,Servo2

; do microsecond part of timing loop. Always takes 32+uS per channel. 
; Each servo pulse starts somewhere (0~31uS) inside its 32uS window.

; servo 1 
		comf	Servo1
		movf	Servo1,w
		andlw	b'00011111'	
		call	usdelay		; delay 31-0uS
		bsf	GPIO,SERVO1	; start servo output pulse
		comf	Servo1
		movf	Servo1,w
		andlw	b'00011111'
		call	usdelay		; delay 0~31uS
; servo 2
		comf	Servo2
		movf	Servo2,w
		andlw	b'00011111'	
		call	usdelay		; delay 31~0uS
		bsf	GPIO,SERVO2	; start servo output pulse
		comf	Servo2
		movf	Servo2,w
		andlw	b'00011111'
		call	usdelay		; delay 0~31uS

; divide pulse widths by 32

		movlw	5	; shift right * 5 = divide by 32
		movwf	t1
servooutdiv:
		rr16	Servo1
		rr16	Servo2
		decfsz	t1,f
		goto	servooutdiv

; do 32us part of timing. Each channel is ended when its time is 
; up, in 32uS steps. 

		movlw 78		; 78*32 = ~2.5mS maximum pulse width	
		movwf t1

servoout32:	decf	Servo1
		skpnz	
		bcf	GPIO,SERVO1	; end servo1 ouput pulse
		decf	Servo2
		skpnz	
		bcf	GPIO,SERVO2	; end servo2 output pulse

		movlw	32-21
		call	usdelay		; wait 32uS (-overhead)

		decfsz	t1
		goto	servoout32	; next 32uS

		bcf	GPIO,SERVO1
		bcf	GPIO,SERVO2	; ensure that outputs are done!
		return


;****************** General purpose functions ****************************

;----------------------------------------------------------------
;                11~266 Microsecond Delay
;----------------------------------------------------------------
; Input: w = delay time in microseconds -11
;
usdelay:	addlw	-4
		skpnc
		goto	usdelay
		sublw	-1
		addwf	PCL,f
		nop
		nop
		nop
		nop
		return


;----------------------------------------------------------------
;               Millisecond Delay
;----------------------------------------------------------------
; Input: w = delay time in microseconds
;
delay1ms:	; 1 millisecond delay
		movlw	-8	; preset countdown value to W
delayx4us:
		addlw	-1	; subtract 1
		btfsc	STATUS,C
		goto	delayx4us
		return


;-------------------------------------------------------------------------------
;                    Millisecond Delay Timer
;-------------------------------------------------------------------------------
; Input: W = number of milliseconds to wait (max 256mS)
;

dx1k:		movwf	Temp1
_dx1k1:		movlw	(1000-5)/5
		movwf	Temp2
_dx1k2:		clrwdt			; avoid watchdog timeout
		nop
		decfsz	Temp2		; wait 1mS
		goto	_dx1k2
		decfsz	Temp1
		goto	_dx1k1
		retlw	0


;*******************************************************************************
;				   Main
;*******************************************************************************


Main:		btfsc	Flags,WATCH	; did the watchdog timeout ?
        	goto    Restart		; oops! try to keep going ...

		clrf	Flags		; clear all flags

		bsf	GPIO,LED	; signal LED on

		movlw	250		; wait 250mS for power to stabilise
		call	dx1k

Restart:	bcf	GPIO,LED	; signal LED off

WaitHigh:	clrwdt			; avoid watchdog timeout
		btfsc	GPIO,PULSE	; wait for start of servo pulse
		goto	WaitHigh
WaitLow:	clrwdt			; avoid watchdog timeout
		btfsc	GPIO,PULSE	; wait for end of servo pulse
		goto	WaitLow

		banksel	IOC
		movlw	b'00001000'	; enable Int On Change GP3
		movwf	IOC
		banksel	0
		bsf	INTCON,GPIE	; enable IOC interrupts

MainLoop:	bcf	T1CON,TMR1ON	; Stop timer1
		bcf	Flags,BADPULSE
		bcf	Flags,GOTPULSE
		bcf	INTCON,T1IF	; clear Timer1 interrupt 
		movf	GPIO,w
		bcf	INTCON,GPIF	; clear pin change int
		bsf	INTCON,GIE	; enable interrupts globally 

; wait for isr to receive servo pulse

wait_pulse:	clrwdt	
		btfsc	Flags,BADPULSE	; isr detected noise ?
		goto	no_signal
		btfss	Flags,GOTPULSE	; isr received pulse ?
		goto	wait_pulse

got_pulse:	clrwdt
		movlw	10
		subwf	PulseCount+1,w
		skpnc			; pulse too long ?
		goto	bad_pulse
		movlw	2
		subwf	PulseCount+1,w
		skpc			; pulse too short ?
		goto	bad_pulse		

		bsf	GPIO,LED	; signal LED on

		mov16	PulseCount,ServoCount

Process:	mov16	ServoCount,Servo1	; output1 = normal
		mov16	ServoCount,aa
		movi16	1500,Servo2		; mid-point = 1.5mS
		sub16	Servo2,aa		
		sub16	aa,Servo2		; output2 = reversed

		btfss	GPIO,JP1	; trimpot enabled?
		goto	do_output	; no,

		call	ReadPot		; yes, get pot position
		mov16	PotVolts,Trim

		ifdef	TRIM1
		add16	Trim,Servo1
		subi16	127,Servo1	; trim output1
		endif

		ifdef	TRIM2
		add16	Trim,Servo2
		subi16	127,Servo2	; trim output2
		endif

do_output:
		call	servo_out	; send pulses to servos

		goto	done

bad_pulse:
no_signal:	clrwdt
		bcf	GPIO,LED	; LED off

; all done, loop back for next input pulse

done:		goto	MainLoop



;------------------ Oscillator Calibration Subroutine -----------------------
;
; Only required if the original factory programmed value has been erased.
;
; NOTE: You should read the factory programmed value and record it, BEFORE 
;       programming your 12F675!

		ifdef	__12F675
		org	0x3ff
		retlw	0x80	; replace with OSCCAL value for your PIC
		endif

		END


