;***************************************************************************
;      MAG2.asm  2 Servos to Magnetic Actuators
;***************************************************************************
;            Bruce Abbott   bhabbott@paradise.net.nz 
;
;      for Microchip PIC12C5xx/12F6xx running at @ 4MHz (INT RC Clock)
;
;===========================================================================
;                            Description
;
; Converts 1~2mS Servo pulse into PWM output, giving proportional control
; of two magnetic actuators.  
;
;
; ==========================================================================
;                          Summary of Changes
;
; 2004/2/2  V0.01 - Initial release 
;
; --------------------------------------------------------------------------
;

#DEFINE version  0
#DEFINE revision 1

;	 PROCESSOR PIC12C508
;        PROCESSOR PIC12F675
	
	ifdef	  __12C508
        INCLUDE   <P12C508.inc>
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_IntRC_OSC	
	else
        INCLUDE   <P12F675.inc>
	__CONFIG  _MCLRE_OFF&_CP_OFF&_WDT_ON&_BODEN_ON&_INTRC_OSC_NOCLKOUT
	endif

         radix     dec

#undefine movfw	; bad, bad macro!

	errorlevel 0,-305,-302

;#DEFINE NO_OSCCAL 	; enable if OSCCAL value was erased!

; Bit definitions for the GPIO register and the TRIS register

#DEFINE LEFT	 0    ; pin 7   
#DEFINE RIGHT	 1    ; pin 6   
#DEFINE DOWN	 2    ; pin 5    
#DEFINE UP	 5    ; pin 2    
#DEFINE CH1_in	 3    ; pin 4   input channel 1 (rudder) 
#DEFINE	CH2_in	 4    ; pin 3   input channel 2 (elevator)

#DEFINE TrisBits (1<<CH1_in|1<<CH2_in)	; Port pins that are inputs

; Bits to be set with the OPTION instruction
;   No wake up
;   No weak pullups
;   Timer 0 source internal
;   Which edge is don't care
;
;   Prescaler to Watchdog, ~550mS timeout.
;
;
#DEFINE OptionBits B'11011101'

;===========================================================================
; Macro to create offsets for variables in RAM
;
	ifdef	__12C508	
ByteAddr	SET	7		; user RAM starts here
	else
ByteAddr	SET	32
	endif	

BYTE            MACRO	ByteName
ByteName        EQU	ByteAddr
ByteAddr	SET	ByteAddr+1
                ENDM

; ==========================================================================
;                 RAM Variable Definitions  
;

; PWM output values, loaded into PWM counters at start of each PWM cycle.

 	BYTE	Start_PWM	; GPIO output at start of PWM cycle 

	BYTE	Right_PWM	; PWM high time for right rudder
	BYTE	Left_PWM	; PWM high time for left rudder
	BYTE	Up_PWM		; PWM high time for up elevator
	BYTE	Down_PWM	; PWM high time for down elevator

; PWM counters 

	BYTE	pwm_period	; counts PWM period time 		

	BYTE	right_count	; counts PWM high time for right rudder
	BYTE	left_count	; counts PWM high time for left rudder
	BYTE	up_count	; counts PWM high time for up elevator
	BYTE	down_count	; counts PWM high time for down elevator
	
; channel inputs

        BYTE	Rudder		; channel 1 (1st channel)      
        BYTE	Elevator	; channel 2 (2nd channel)

; other variables

        BYTE	Flags		; various boolean flags            

	BYTE	temp_1		; temporary storage 	
	BYTE	temp_2
        BYTE	temp_3		 


#DEFINE PWMSTEPS  16		; no. of steps in a PWM cycle

;
; flag values
;
#DEFINE TIMEOUT	0		; watchdog timed out

;
; constants
;


;***************************************************************************
;                                Code
;***************************************************************************

	ORG	0
	goto	ColdStart

;-------------------------- version string ---------------------------------

	org	8		
	dt	"--MAG2--"
	dt	"--V"
	dw	version+"0"
	dt	"."
	dw	revision+"0"
	dt	"--"

; =============================================+============================
; 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

;-----------------------------------------------------------------------
;                      Generate PWM output
;-----------------------------------------------------------------------
;
; Takes 18uS (including call&return) 
;
; Must be called every 26uS.   
;
;
;
DoPWM:		decfsz	pwm_period	;3    end of PWM cycle ?
		goto	pwm_step	;4(5) no, do the next step
		movf	Left_PWM,w	;5   yes, 
		movwf	left_count	;6
		movf	Right_PWM,w	;7
		movwf	right_count	;8
		movf	Up_PWM,w	;9  reload all counters
		movwf	up_count	;10
		movf	Down_PWM,w	;11
		movwf	down_count	;12
		movlw	PWMSTEPS	;13
		movwf	pwm_period	;14
		movf	Start_PWM,w	;15 restart active PWMs
		movwf	GPIO		;16
		retlw	0		;17,18
		
pwm_step:	movlw	0		;6  Assume all PWMs will be OFF
		decfsz	left_count	;7  Count down. If count<>0 then  
		iorlw	1<<LEFT		;8  PWM output is allowed ON.
		decfsz	right_count	;9 
		iorlw	1<<RIGHT	;10
		decfsz	up_count	;11
		iorlw	1<<UP		;12
		decfsz	down_count	;13
		iorlw	1<<DOWN		;14
		nop			;15   Turn off any PWMs that have
		andwf	GPIO		;16   just timed out. 
		retlw	0		;17,18

		



;============================================================================

ColdStart:		
	bcf	Flags,TIMEOUT
	btfss	STATUS,NOT_TO		; did Watchdog time out ? 
	bsf	Flags,TIMEOUT

	ifdef	__12C508
	ifdef	NO_OSCCAL		
	movlw	0x90			; replace with value for your PIC!
	endif
	movwf	OSCCAL			; set oscillator calibration 
	else 
	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
                 
; Move prescaler from tmr0 to the watchdog, without causing accidental reset!

        clrwdt                    
        clrf	TMR0            
        movlw	OptionBits | 7  
        option                    	
        clrwdt
        movlw	OptionBits
        option
        clrwdt

; initialise I/O registers 

        clrf	GPIO			; all outputs low
	ifdef	__12C508
	movlw	TrisBits
	Tris	GPIO
	else
	bsf	STATUS,RP0		; register bank 1 
        movlw	TrisBits
        movwf	TRISIO			; set I/O pin directions
	ifdef	ANSEL
	clrf	ANSEL			; disable analog inputs (12F675)
	endif
	bcf	STATUS,RP0		; register bank 0
	movlw	b'00000111'		      
        movwf	CMCON			; Comparator off
	endif

; CPU specific stuff done, now we can start the main program!

       goto	Main		
							                               


;-------------------------------------------------------------------------------
;                    Millisecond Delay Timer 
;-------------------------------------------------------------------------------
; Input: W = number of milliseconds to wait x 2 (max 512mS)
;
dx2k:		movwf	temp_1		
_dx2k1:		movlw	(2000-2)/10	
		movwf	temp_2		
_dx2k2:		clrwdt			; avoid watchdog timeout			;1
		no_op	6						
		decfsz	temp_2		
		goto	_dx2k2		
		decfsz	temp_1		
		goto	_dx2k1		
		retlw	0		


;***************************************************************************
;				   Main
;***************************************************************************		


Main:		btfsc	Flags,TIMEOUT	; did the watchdog timeout ?
        	goto    Start		; oops! Try to recover...

		movlw	250		; wait 500mS for Rx to stabilise 
		call	dx2k		; 

Start:		clrf	Flags		; clear all flags

		clrf	Left_PWM	; all PWM off
		clrf	Right_PWM
		clrf	Up_PWM
		clrf	Down_PWM
		clrf	Start_PWM
		movlw	1
		movwf	pwm_period	; 1st DoPWM inits all counters	

wait1:		clrwdt			;4
		no_op	4		;5~8
		call	DoPWM
		btfsc	GPIO,CH1_in	;1     wait for CH1 Low
		goto	wait1		;2(3)
		nop			;3
start1:		clrwdt			;4
		no_op	4		;5~8
		call	DoPWM
		btfss	GPIO,CH1_in	;1     wait for CH1 High
		goto	start1		;2(3)
		
		clrf	Rudder		;3
		no_op	2		;4,5
time1:		clrwdt			;6
		no_op	2		;7,8
		call	DoPWM
		incfsz	Rudder,w	;1
		incf	Rudder		;2     measure length of CH1 pulse
		btfsc	GPIO,CH1_in	;3
		goto	time1		;4(5)

start2:		clrwdt			;5
		no_op	3		;6~8
		call	DoPWM
		nop			;1
		btfss	GPIO,CH2_in	;2     wait for CH2 High
		goto	start2		;3(4)

		clrf	Elevator	;4
		nop			;5
time2:		clrwdt			;6
		no_op	2		;7,8
		call	DoPWM
		incfsz	Elevator,w	;1
		incf	Elevator	;2     measure length of CH2 pulse
		btfsc	GPIO,CH2_in	;3
		goto	time2		;4(5)		
		no_op	3		;6~8
		call	DoPWM		
;
; limit range to 1.1~1.9mS   
;
limit:		movlw	1100/26		;1
		subwf	Rudder		;2 
		skpc			;3
		clrf	Rudder		;4  1.1mS minimum
		no_op	4		;5~8
		call	DoPWM		
		movlw	1100/26		;1
		subwf	Elevator	;2 
		skpc			;3
		clrf	Elevator	;4  1.1mS minimum
		no_op	2		;5,6
;
; wait until the next PWM cycle starts. 
;
wait_pwm:	no_op	2		;7,8
		call	DoPWM
		decf	pwm_period,w	;1
		skpnz			;2
		goto	last_pwm	;3(4)
		nop			;4
		goto	wait_pwm	;5,6

last_pwm:	no_op	4		;5~8
		call	DoPWM		;
				
;
; Update PWM variables with new values, whilst completing the current PWM 
; cycle with old values. 
;
; split rudder and elevator into bi-directional PWM
;
;  1~15 = low      PWM = 15~1
;    16 = center   PWM = 0
; 17~31 = high     PWM = 1~15
; 
Do_Rudder:	movlw	16		;1
		subwf	Rudder,w	;2 right or left ?		
		skpc			;3
		goto	do_left		;4(5)

do_right:	movwf	Right_PWM	;5 right PWM = 0~15
		clrf	Left_PWM	;6 left PWM = 0
		no_op	2		;7,8
		call	DoPWM
		no_op	6		;1~6
		goto	do_elevator	;7,8

do_left:	clrf	Right_PWM	;6 right PWM = 0
		no_op	2		;7,8
		call	DoPWM
		movlw	16		;1
		movwf	Left_PWM	;2
		movf	Rudder,w	;3 left PWM = 16~1
		subwf	Left_PWM	;4		
		no_op	4		;5~8
do_elevator:	call	DoPWM
		movlw	16		;1		
		subwf	Elevator,w	;2  
		skpc			;3
		goto	do_down		;4(5)

do_up:		movwf	Up_PWM		;5 
		clrf	Down_PWM	;6 
		no_op	2		;7,8
		call	DoPWM
		no_op	3		;1~3
		goto	update		;4,5

do_down:	clrf	Up_PWM		;6
		no_op	2		;7,8
		call	DoPWM
		movlw	16		;1
		movwf	Down_PWM	;2
		movf	Elevator,w	;3
		subwf	Down_PWM	;4
		nop			;5
;
; Set starting state for each PWM output.
;
update:		clrf	Start_PWM	;6 Assume all PWM outputs are OFF 
		no_op	2		;7,8
		call	DoPWM
		tstf	Left_PWM	;1 PWM count = 0 ?
		skpz			;2
		bsf	Start_PWM,LEFT	;3 no, PWM output will be ON
		tstf	Right_PWM	;4
		skpz			;5
		bsf	Start_PWM,RIGHT	;6
		no_op	2		;7,8
		call	DoPWM	
		tstf	Down_PWM	;1
		skpz			;2
		bsf	Start_PWM,DOWN	;3
		tstf	Up_PWM		;4
		skpz			;5
		bsf	Start_PWM,UP	;6
		no_op	2		;7,8
		call	DoPWM
		nop			;1
		goto	wait1		;2,3 wait for next input




;---------- Oscillator Calibration Subroutine --------------


		ifdef	NO_OSCCAL
		org	0x3ff
		ifdef	__12F675
		retlw	0x70	; replace with osccal value for your 12F675!	
		endif
		endif

		END