256 lines
8.8 KiB
Plaintext
Executable File
256 lines
8.8 KiB
Plaintext
Executable File
;==========================================================================================================
|
|
;
|
|
; 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
|
|
;==========================================================================================================
|