301 lines
11 KiB
NASM
Executable File
301 lines
11 KiB
NASM
Executable File
;==========================================================================================================
|
|
;
|
|
; 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
|
|
;==========================================================================================================
|