;==========================================================================================================
; 
; Wingman MainMotorDriver	 					copyright 2005				  	          v1.0 (2005-04-16)
; 
; Name: Christoffer Martinsson
; E-mail: cm@wsm.se 
;
; 			
;==========================================================================================================

include P16F684.INC
			
W_TEMP		EQU		0x20			; TempRegister
STATUS_TEMP	EQU		0x21			; TempRegister

PPM_ValueH	EQU		0x22
PPM_ValueL	EQU		0x23

PulseInH	EQU		0x24
PulseInL	EQU		0x25

;==========================================================================================================
; Reset- and Interrupt-vectors
;==========================================================================================================
        	ORG     0x0000               
			GOTO 	Main			;Reset-vector
			ORG		0x0004
			GOTO	Interrupt		;Interrupt-vector	
;==========================================================================================================
; Macro
;==========================================================================================================
;----------------------------------------------------------------------------------------------------------
; StoreWregInTemp 
; 
; Description: Store Status and working-register in temp-registers
;
; Input: -
; ChangedReg: -
; UsedReg: W,STATUS,W_TEMP,STATUS_TEMP
;----------------------------------------------------------------------------------------------------------
StoreWregInTemp	MACRO
			MOVWF	W_TEMP				;Copy W to TEMP register
			SWAPF 	STATUS,W 			;Swap status to be saved into W
			CLRF 	STATUS 				;bank 0, regardless of current bank, Clears IRP,RP1,RP0
			MOVWF 	STATUS_TEMP 		;Save status to bank zero STATUS_TEMP register
			ENDM
;----------------------------------------------------------------------------------------------------------
; RestoreWregFromTemp
; 
; Description: Restore Status and working-register from temp-registers
;
; Input: -
; ChangedReg: W,STATUS
; UsedReg: W,STATUS,W_TEMP,STATUS_TEMP
;----------------------------------------------------------------------------------------------------------
RestoreWregFromTemp	MACRO
			SWAPF 	STATUS_TEMP,W 		;Swap STATUS_TEMP register into W (sets bank to original state)
			MOVWF 	STATUS 				;Move W into Status register
			SWAPF 	W_TEMP,F 			;Swap W_TEMP
			SWAPF 	W_TEMP,W 			;Swap W_TEMP into W
			ENDM
;----------------------------------------------------------------------------------------------------------
; Compare16
; 
; Description: Compare two 16bit-registers			if Reg1=Reg2 then Z=1.
;													if Reg1<Reg2 then C=1.
;													if Reg2<Reg1 then C=0.
; Input: Reg1H, Reg1L, Reg2H, Reg2L
; ChangedReg: Z, C
; UsedReg: W
;----------------------------------------------------------------------------------------------------------
Compare16	MACRO 	Reg1H, Reg1L, Reg2H, Reg2L
			MOVF 	Reg1H,W
			SUBWF 	Reg2H,W 															
			BTFSS   STATUS,Z									
			GOTO 	Comp16End										
			MOVF 	Reg1L,W							
			SUBWF 	Reg2L,W							
Comp16End	ENDM

;----------------------------------------------------------------------------------------------------------
; Sub16
; 
; Description: Subtract 16bit-register.	Reg1 = Reg1 - Reg2, WITH VALID CARRY
;
; Input: Reg1H, Reg1L, Reg2H, Reg2L
; ChangedReg: C
; UsedReg: W
;----------------------------------------------------------------------------------------------------------
Sub16		MACRO 	Reg1H, Reg1L, Reg2H, Reg2L
			MOVF    Reg2L,W
        	SUBWF   Reg1L
        	MOVF    Reg2H,W
        	BTFSS   STATUS,C
        	INCFSZ  Reg2H,W
        	SUBWF   Reg1H           
			ENDM
;----------------------------------------------------------------------------------------------------------
; Sub16L
; 
; Description: Subtract 16bit-register.	Reg1 = Reg1 - Number, WITH VALID CARRY
;
; Input: Reg1H, Reg1L, Number
; ChangedReg: C
; UsedReg: W
;----------------------------------------------------------------------------------------------------------
Sub16L		MACRO 	Reg1H, Reg1L, Number
			MOVF    low(Number),W
        	SUBWF   Reg1L
        	MOVF    high(Number),W
        	BTFSS   STATUS,C
        	INCFSZ  high(Number),W
        	SUBWF   Reg1H           
			ENDM
;----------------------------------------------------------------------------------------------------------
; Set16L
; 
; Description: Sets 16bit-register to the value of 'Number'. Reg1 = Number
;
; Input: Reg1H, Reg1L, Number
; ChangedReg: C
; UsedReg: W
;----------------------------------------------------------------------------------------------------------
Set16L		MACRO 	Reg1H, Reg1L, Number
			MOVLW   low(Number)
        	MOVWF	Reg1L
        	MOVLW   high(Number)
        	MOVWF   Reg1H           
			ENDM
;----------------------------------------------------------------------------------------------------------
; Clear16
; 
; Description: Clear 16bit-register.
;
; Input: RegH, RegL
; ChangedReg:
; UsedReg:
;----------------------------------------------------------------------------------------------------------
Clear16		MACRO 	RegH, RegL
			CLRF   	RegH
			CLRF	RegL         
			ENDM
;----------------------------------------------------------------------------------------------------------
; Comp16L
; 
; Description: Compare one 16bit-registers to a literal		if Reg1=Number then Z=1.
;															if Reg1<Number then C=1.
;															if Number<Reg1 then C=0.
; Input: Reg1H, Reg1L, Number
; ChangedReg: Z, C
; UsedReg: W
;----------------------------------------------------------------------------------------------------------
Comp16L		MACRO 	Reg1H, Reg1L, Number
			MOVF 	Reg1H,W
			SUBWF 	high Number,W 															
			BTFSS   STATUS,Z									
			EXITM										
			MOVF 	Reg1L,W							
			SUBWF 	low Number,W								
			ENDM
;==========================================================================================================
; Interrupt routines
;==========================================================================================================
Interrupt	StoreWregInTemp
;----------------------------------------------------------------------------------------------------------
; Timer1_Int  
; 
; Description: Increment pulseIn counter-registers              
;----------------------------------------------------------------------------------------------------------
Timer1_Int	BCF 		STATUS,RP0 						; Bank 0
			BTFSS		PIR1,TMR1IF
			GOTO		Int_Return
			BCF			PIR1,TMR1IF
			BCF			T1CON,TMR1ON					; Timer1 off

Int_Return	RestoreWregFromTemp
			RETFIE

;==========================================================================================================
; Main
;==========================================================================================================
Main		BCF 		STATUS,RP0 						; Bank 0
			CLRF 		PORTC							; Clear PORTC
			CLRF		PORTA							; Clear PORTA
			MOVLW 		0x07 				
			MOVWF 		CMCON0 							; Comparator OFF
			CLRF		CCPR1L
			MOVLW		b'01001100'
			MOVWF		CCP1CON		
			MOVLW		b'10000001'
			MOVWF		ADCON0	
		;	BSF			INTCON,INTE			


			BSF 		STATUS,RP0						; Bank 1
			MOVLW		b'00001101'
			MOVWF		TRISA				
			CLRF		TRISC
			MOVLW		b'00000001'
			MOVWF 		ANSEL 							; All pins to digital I/O except AN0			
			MOVLW		0x3F
			MOVWF		PR2		
			MOVLW		b'00100000'
			MOVWF		ADCON1
			MOVLW		b'11001111'
			MOVWF		OPTION_REG

			

			BCF 		STATUS,RP0 						; Bank 0
			BSF			T2CON,TMR2ON					; PWM on
			BCF			INTCON,GIE						; Global interrup enable

;----------------------------------------------------------------------------------------------------------
; MainLoop
;----------------------------------------------------------------------------------------------------------
Mainloop:	BCF 		STATUS,RP0 						; Bank 0
			CLRWDT
			
;DECODE_PPM		
PPM_Lo:		BTFSC		PORTA,2
			GOTO		PPM_Lo
PPM_Hi:		BTFSS		PORTA,2
			GOTO		PPM_Hi
			Clear16		TMR1H,TMR1L
			BSF			T1CON,TMR1ON					; Timer1 on
PPM_Loop:  	BTFSC		PORTA,2       		
          	GOTO    	PPM_Loop           	
          	BCF			T1CON,TMR1ON					; Timer1 off		
			Comp16L 	TMR1H,TMR1L,.20000
			BTFSC		STATUS,C
			GOTO		Mainloop
			Comp16L		TMR1H,TMR1L,.40000
			BTFSS		STATUS,C
			GOTO		Mainloop
			Sub16L		TMR1H,TMR1L,.20000
			Comp16L		TMR1H,TMR1L,.10000
			BTFSS		STATUS,C
			GOTO		PWM_Fwr
			GOTO		PWM_Rev

PWM_Fwr:	BCF			CCP1CON,P1M1
			Sub16L		TMR1H,TMR1L,.10000
			GOTO		PWM_SetVal
PWM_Rev:	BSF			CCP1CON,P1M1
			Sub16L		TMR1H,TMR1L,.10000
		
PWM_SetVal:	


			GOTO		Mainloop
			END
;==========================================================================================================
; END
;==========================================================================================================