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

include P16F684.INC

; Temp-registers			
W_TEMP		EQU		0x20			; TempRegister for interrupt-routine
STATUS_TEMP	EQU		0x21			; TempRegister for interrupt-routine
DivRegH		EQU		0x22			; TempRegister for div-routine
DivRegL		EQU		0x23			; TempRegister for div-routine
SetPWM_Temp	EQU		0x24

; Time-definitions
minPulseValue 		=.20000
maxPulseValue		=.40000
ms1Value			=.20000
ms05Value			=.10000

;==========================================================================================================
; 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: W_TEMP,STATUS_TEMP
; 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
;----------------------------------------------------------------------------------------------------------
; Sub16L
; 
; Description: Subtract literal from 16bit-register.	Reg = Reg - Number, WITH VALID CARRY
;
; Input: RegH, RegL, Number
; ChangedReg: C
; UsedReg: W
;----------------------------------------------------------------------------------------------------------
Sub16L		MACRO 	RegH, RegL, Number
			MOVLW   low Number
        	SUBWF   RegL
        	MOVLW   high Number
        	;BTFSS   STATUS,C
        	;INCFSZ  high Number,W
        	SUBWF   RegH           
			ENDM
;----------------------------------------------------------------------------------------------------------
; Set16L
; 
; Description: Sets 16bit-register to literal. Reg = Number
;
; Input: RegH, RegL, Number
; ChangedReg: C
; UsedReg: W
;----------------------------------------------------------------------------------------------------------
Set16L		MACRO 	RegH, RegL, Number
			MOVLW   low Number
        	MOVWF	RegL
        	MOVLW   high Number
        	MOVWF   RegH           
			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 literal		if Reg1=Number then Z=1.
;															if Reg1<Number then C=1.
;															if Number<Reg1 then C=0.
; Input: RegH, RegL, Number
; ChangedReg: Z, C
; UsedReg: W
;----------------------------------------------------------------------------------------------------------
Comp16L		MACRO 	RegH, RegL, Number
			MOVF 	RegH,W
			SUBLW 	high Number															
			BTFSS   STATUS,Z									
			EXITM										
			MOVF 	RegL,W							
			SUBLW 	low Number								
			ENDM
;----------------------------------------------------------------------------------------------------------
; SetPWM
; 
; Description: Sets hardware PWM-output to 8bit register-value. PWM = Reg
;															
; Input: Reg
; ChangedReg: CCP1CON(4,5), CCPR1L
; UsedReg: W, CCP1CON(4,5), CCPR1L, Reg, SetPWM_Temp
;----------------------------------------------------------------------------------------------------------
SetPWM		MACRO 	Reg
			BCF		STATUS,C
			MOVF	Reg,W
			ANDLW	b'00000011'
			MOVWF	SetPWM_Temp
			RLF		SetPWM_Temp,F
			RLF		SetPWM_Temp,F
			RLF		SetPWM_Temp,F
			RLF		SetPWM_Temp,F
			MOVF	CCP1CON,W
			ANDLW	b'11001111'
			IORWF	SetPWM_Temp,W
			MOVWF	CCP1CON	
			MOVF	Reg,W
			ANDLW	b'11111100'
			MOVWF	SetPWM_Temp
			RRF		SetPWM_Temp,F
			RRF		SetPWM_Temp,F
			MOVF	SetPWM_Temp,W
			MOVWF	CCPR1L		
			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

;==========================================================================================================
; Sub-routines
;==========================================================================================================
;----------------------------------------------------------------------------------------------------------
; Div16byL
; 
; Description: Divide a 16bit-register with a literal.		DivReg = DivReg / W.
;															
; Input: DivRegH, DivRegL, W
; ChangedReg: Z, C, DivRegH, DivRegL
; UsedReg: W, DivRegH, DivRegL
;----------------------------------------------------------------------------------------------------------
Div16byL	CALL 		DivSkipHiShift
			iDivRepeat	=8
			WHILE 		iDivRepeat
    		call 		DivCode
			iDivRepeat--
			ENDW
			RLF 		DivRegL,F 						; C << lo << C
			BCF			STATUS,Z
    		RETURN				      					; we are done!

DivCode	    RLF 		DivRegL,F  	  					; C << lo << C
    		RLF 		DivRegH,F    					; C << hi << C
    		BTFSS 		STATUS,C	    				; if Carry
     		GOTO 		DivSkipHiShift 
    		SUBWF 		DivRegH,F  						; hi-=w
    		BSF			STATUS,C        				; ignore carry
    		RETURN				        				; done
                         								; endif
DivSkipHiShift
    		SUBWF 		DivRegH,F						; hi-=w
    		BTFSC 		STATUS,C       					; if carry set
     		RETURN 	       								; done
    		ADDWF 		DivRegH,F  						; hi+=w
    		BCF			STATUS,C        				; clear carry
			RETURN
;==========================================================================================================
; 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 		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
			BSF			INTCON,GIE						; Global interrup enable

;			MOVLW		low .30000
;			MOVWF		TMR1L
;			MOVLW		high .30000
;			MOVWF		TMR1H			

;----------------------------------------------------------------------------------------------------------
; MainLoop
;----------------------------------------------------------------------------------------------------------
Mainloop:	BCF 		STATUS,RP0 						; Bank 0
			CLRWDT
			
;DECODE_PPM		
PPM_Lo:		BTFSC		PORTA,2							; Wait for pulse to go low 							
			GOTO		PPM_Lo
PPM_Hi:		BTFSS		PORTA,2							; Wait for pulse to go high (start measuring) 
			GOTO		PPM_Hi
			Clear16		TMR1H,TMR1L						; Clear Timer1 countervalue
			BSF			T1CON,TMR1ON					; Start Timer1
PPM_Loop:  	BTFSC		PORTA,2       					; Wait for pulse to go low (stop measuring) 
          	GOTO    	PPM_Loop           	
          	BCF			T1CON,TMR1ON					; Stop Timer1 		
	;		Comp16L 	TMR1H,TMR1L,minPulseValue		
	;		BTFSC		STATUS,C						; If counter < minPulseValue (~1ms)
	;		GOTO		Mainloop						; then goto Mainloop
	;		Comp16L		TMR1H,TMR1L,maxPulseValue		
	;		BTFSS		STATUS,C						; else If counter > maxPulseValue (~2ms)
	;		GOTO		Mainloop						; then goto Mainloop
			Sub16L		TMR1H,TMR1L,ms1Value			; else counter-value - 1ms
			Comp16L		TMR1H,TMR1L,ms05Value			
			BTFSS		STATUS,C						; If counter > 0,5ms
			GOTO		PWM_Fwr							; then goto PWM_forward
			GOTO		PWM_Rev							; else goto PWN_reverse

PWM_Fwr:	BCF			CCP1CON,P1M1					; Set H-bridge in forward-mode
			Sub16L		TMR1H,TMR1L,ms05Value			; counter-value - 0,5ms
			GOTO		PWM_SetVal						; goto PWM_SetValue
PWM_Rev:	BSF			CCP1CON,P1M1					; Set H-bridge in reverse-mode
			MOVLW		0xFF
			XORWF		TMR1H,F							; Invert counterH
			XORWF		TMR1L,F							; Invert counterL
			Sub16L		TMR1H,TMR1L,(0xFFFF-ms05Value)	; Inverted counter-value - (65535-0,5ms)
		
PWM_SetVal:	MOVF		TMR1H,W							; Divide by 79...
			MOVWF		DivRegH							; Load div-routine with countervalue
			MOVF		TMR1L,W
			MOVWF		DivRegL							; Load div-routine with countervalue
			MOVLW		((ms05Value/0xFF)+1)			; Load div-routine with divide-value 
			CALL		Div16byL						; Divide!
			
			SetPWM		DivRegL

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