📄 snsles2431.asm
字号:
;The switching sequence is updated in this routine
UPDATE_SEQUENCE
;init table again
btfss FLAGS1,FWD_REV
bra ITS_REVERSE
lfsr 0,POSITION_TABLE_FWD
bra PICK_FROM_TABLE
ITS_REVERSE
lfsr 0,POSITION_TABLE_REV
;--
PICK_FROM_TABLE
movf PORTA,W
comf WREG,W ;In forward direction, the BEMF ZC is inverted
andlw 0x1C ;IC1/IC2/IC3
rrncf WREG,W
rrncf WREG,W
movwf BEMF_ZC
; movf PLUSW0,W
movwf OVDCOND_TEMP
movlw 0x00
cpfseq OVDCOND_TEMP
bra TEST
;return
call LED_Display
return
TEST
movlw 0x07
cpfseq OVDCOND_TEMP
return
call LED_Display
return
;-------------------------------------------------------------
;-------------------------------------------------------------
;-------------------------------------------------------------
;-------------------------------------------------------------
;Timer1 overflow interrupt
TIMER1_INT
bcf PIR1,TMR1IF
movlw 0x08
cpfsgt CX4
bra OPEN_LOOP_ACTIVE
movlw 0x09
movwf CX4
bra SENSERLESS_T0INT
OPEN_LOOP_ACTIVE
bcf FLAGS_SRLS,IC_ALOWED
btfsc FLAGS_SRLS,OPEN_SPEED_STEP2
bra OPEN_SPEED_STEP_2
movlw LOW_OL_SPEEDH ;0xD8F0 = 1.92mSec@20MHz/1000RPM
movwf TMR1H
movlw LOW_OL_SPEEDL
movwf TMR1L
call SET_SEQUENCE_COUNT
infsnz COUNT_OPEN_LOOPL,F
bsf FLAGS_SRLS,OPEN_SPEED_STEP2
retfie FAST
OPEN_SPEED_STEP_2
movlw 0x09
cpfseq CX1
goto S1
incf CX3
movlw 0xf0
cpfseq CX3
goto S2
incf CX4
clrf CX3
movlw 0xf0
cpfseq CX4
goto S2
movlw 0xef
movwf CX4
S2 goto T3100
S1 incf CX
movlw 0xf0
cpfseq CX
goto T00
goto STEP1
STEP1
incf CX1
clrf CX
movlw 0x01
cpfseq CX1
goto Continue
goto T800
Continue
movff CX1,CX2
decf CX2
movf CX2,W
mullw 0x03
movf CX1,W
addwf PRODL,W
call CONVERT
movwf HIGH_OL_SPEEDH
movff CX1,CX2
decf CX2
movf CX2,W
mullw 0x03
movf CX1,W
addwf PRODL,W
addlw 0x01
call CONVERT
movwf HIGH_OL_SPEEDL
bra T00
T800
movlw 0x85
movwf HIGH_OL_SPEEDH
movlw 0xed
movwf HIGH_OL_SPEEDL
goto T00
T3100
movlw 0xe0
movwf HIGH_OL_SPEEDH
movlw 0x7e
movwf HIGH_OL_SPEEDL
;call UPDATE_SEQUENCE
goto T00
T00
movff HIGH_OL_SPEEDH, TMR1H
movff HIGH_OL_SPEEDL, TMR1L
call SET_SEQUENCE_COUNT
retfie FAST
SENSERLESS_T0INT
bcf PIE3,IC1IE
; call UPDATE_SEQUENCE
; movlw 0xe4
; movwf HIGH_OL_SPEEDH ;1000RPM
; movlw 0x18
; movwf HIGH_OL_SPEEDL
; movff HIGH_OL_SPEEDH, TMR1H
; movff HIGH_OL_SPEEDL, TMR1L
; call SET_SEQUENCE_COUNT
; retfie FAST
call UPDATE_SEQUENCE
bcf PIE3,IC1IE
bsf FLAGS_SRLS,SWITCH_SENSORLESS
btfss FLAGS1,FWD_REV
goto REV
goto FWD
REV
movlw 0x06
cpfseq BEMF_ZC
bra Y1
movlw b'00000010'
movwf OVDCONS
movlw b'00000100'
movwf OVDCOND
goto Set_Time
Y1
movlw 0x05
cpfseq BEMF_ZC
bra Y2
movlw b'00001000'
movwf OVDCONS
movlw b'00010000'
movwf OVDCOND
goto Set_Time
Y2
movlw 0x04
cpfseq BEMF_ZC
bra Y3
movlw b'00000010'
movwf OVDCONS
movlw b'00010000'
movwf OVDCOND
goto Set_Time
Y3
movlw 0x03
cpfseq BEMF_ZC
bra Y4
movlw b'00100000'
movwf OVDCONS
movlw b'00000001'
movwf OVDCOND
goto Set_Time
Y4
movlw 0x02
cpfseq BEMF_ZC
bra Y5
movlw b'00100000'
movwf OVDCONS
movlw b'00000100'
movwf OVDCOND
goto Set_Time
Y5
movlw 0x01
cpfseq BEMF_ZC
return
movlw b'00001000'
movwf OVDCONS
movlw b'00000001'
movwf OVDCOND
goto Set_Time
FWD
movlw 0x01
cpfseq BEMF_ZC
bra J1
movlw b'00100000'
movwf OVDCONS
movlw b'00000001'
movwf OVDCOND
goto Set_Time
J1
movlw 0x02
cpfseq BEMF_ZC
bra J2
movlw b'00000010'
movwf OVDCONS
movlw b'00000100'
movwf OVDCOND
goto Set_Time
J2
movlw 0x03
cpfseq BEMF_ZC
bra J3
movlw b'00100000'
movwf OVDCONS
movlw b'00000100'
movwf OVDCOND
goto Set_Time
J3
movlw 0x04
cpfseq BEMF_ZC
bra J4
movlw b'00001000'
movwf OVDCONS
movlw b'00010000'
movwf OVDCOND
goto Set_Time
J4
movlw 0x05
cpfseq BEMF_ZC
bra J5
movlw b'00001000'
movwf OVDCONS
movlw b'00000001'
movwf OVDCOND
goto Set_Time
J5
movlw 0x06
cpfseq BEMF_ZC
retfie FAST
movlw b'00000010'
movwf OVDCONS
movlw b'00010000'
movwf OVDCOND
goto Set_Time
Set_Time
movff VELOCITY_READH,TMR1H
movff VELOCITY_READL,TMR1L
;movlw 0xe0
;movwf TMR1H
;movlw 0x7e
;movwf TMR1L
BLOCK_IC_OVER
bsf PIE3,IC1IE ;Cap1 interrupt
bsf PIE1,TMR1IE
retfie FAST
;******************************************************************
ISR_LOW
RETFIE
;******************************************************************
;Check overcurrent fault in this routine for 'n' number of PWM cycles in one Timer1 cycle,
;and if the fault persists, the input type is changed to catestrophic mode.
;
PWM_INTERRUPT
bcf PIR3,PTIF
retfie FAST
infsnz LOCK_ROTOR_COUNT,F
clrf OVDCOND
CHECK_OTHER_FAULTS
incfsz PWM_CYCLE_COUNT,F
bra CHECK_FOR_FAULTS
clrf FAULTA_COUNT
bra CHECK_PARAMETER_DISPLAY
CHECK_FOR_FAULTS
btfsc PORTC,CURRENT_FAULT_INPUT
bra CHECK_PARAMETER_DISPLAY
incf FAULTA_COUNT,F
movlw MAX_FLTA_COUNT
cpfsgt FAULTA_COUNT
bra CHECK_PARAMETER_DISPLAY
bcf FLTCONFIG,FLTAMOD
bsf FLTCONFIG,FLTAEN
bsf FLT_FLAGS,OCUR
;----------------------------------Old code
CHECK_PARAMETER_DISPLAY
retfie FAST
movlw CYCLE_COUNT_MAXH
cpfseq CYCLE_COUNTH
bra NOT_YET_THERE
movlw CYCLE_COUNT_MAXL
cpfsgt CYCLE_COUNTL
bra NOT_YET_THERE
bsf FLAGS,PARAM_DISPLAY
clrf CYCLE_COUNTH
clrf CYCLE_COUNTL
btfsc FLT_FLAGS,OCUR
btg LED1
bcf PIR3,PTIF
retfie FAST
NOT_YET_THERE
incfsz CYCLE_COUNTL,F
retfie FAST
incf CYCLE_COUNTH,F
bcf PIR3,PTIF
retfie FAST
;******************************************************************
;PWM duty cycle is updated according to the potentiometer level
UPDATE_PWM
movlw 0x20 ;Check lower limit for PWM
cpfsgt SPEED_REFH
bra SMALL
movlw 0xFA ;Check upper limit to the speed
cpfslt SPEED_REFH
movwf SPEED_REFH
call CALCULATE_TIME_60DEG
;PWM = [(MotorVoltage/DCbus voltage)*(PTPER*4)]*[SpeedRef/255] *16
;16 is the multiplication factor
movf SPEED_REFH,W
mullw (MAIN_PWM_CONSTANT)
swapf PRODL,W
andlw 0x0F
movwf PDC_TEMPL
swapf PRODH,W
andlw 0xF0
iorwf PDC_TEMPL,F
swapf PRODH,W
andlw 0x0F
movwf PDC_TEMPH
bra SET_PDC
SMALL
call CALCULATE_TIME_60DEG
movlw 0x9f
movwf PDC_TEMPL
movlw 0x00
movwf PDC_TEMPH
SET_PDC
bsf PWMCON1,UDIS ;Disable the PWM buffer update
movf PDC_TEMPH,W
movwf PDC0H
movwf PDC1H
movwf PDC2H
movwf PDC3H
movf PDC_TEMPL,W
movwf PDC0L
movwf PDC1L
movwf PDC2L
movwf PDC3L
bcf PWMCON1,UDIS ;Disable the PWM buffer update
RETURN
;---------------------------------------------
RESET_DUTY_CYCLE
clrf PDC0H
clrf PDC1H
clrf PDC2H
clrf PDC3H
clrf PDC0L
clrf PDC1L
clrf PDC2L
clrf PDC3L
RETURN
;---------------------------------------------
;This routine calculates a 60 electrical deg delay
CALCULATE_TIME_60DEG
; call LED_Display
movlw 0x20 ;Check lower limit for PWM
cpfsgt SPEED_REFH
bra SMA
movlw 0x20
subwf SPEED_REFH,W ;offset
mullw 0x71 ;39.2*4
bcf STATUS,C
rrcf PRODH,F ;/4
rrcf PRODL,F
bcf STATUS,C
rrcf PRODH,F
rrcf PRODL,F
;FFFF - (10,000 - 39.2 X (ADC-64h))
bcf STATUS,C
movlw 0x7e
addwfc PRODL,F
movlw 0xe0
addwfc PRODH,F
bra SET_TIME
SMA
movlw 0xe0
movwf VELOCITY_READH
movlw 0x7e
movwf VELOCITY_READL
return
SET_TIME
movff PRODH,VELOCITY_READH
movff PRODL,VELOCITY_READL
return
;************************************************
FAULTA_PROCESS
bsf LED1
bsf LED2
bsf LED3
bsf FLAGS,FLAG_FAULT
call STOP_MOTOR
bcf FLTCONFIG,FLTAS
bcf FLT_FLAGS,OCUR
bcf FLAGS1,KEY_PRESSED
bcf FLAGS1,KEY_RS
bcf FLAGS1,RUN_STOP
return
;******************************************************************
;Peripherals are initialized in this routine
;ADC,PWM, IC, ports and interrupts etc are initialized
INIT_PERPHERALS
;Initialize ADC
movlw b'00010001' ;single shot 1a 2b
movwf ADCON0
movlw b'00010000' ;buffer0 avdd avss for ref fifo
movwf ADCON1
movlw b'00110010' ;left justifed 12TAD acquisition Fosc/32
movwf ADCON2
movlw b'10000000' ;interrupt on 4th ad results to fifo no trigger
movwf ADCON3
movlw b'00000000' ;select an0-an3
movwf ADCHS
movlw b'00000011' ;only use an0 an1, others are digital io
movwf ANSEL0
movlw b'00000000'
movwf ANSEL1
;-----------------------------------------------------------------
;Initialize PWM
movlw b'00000000'
movwf PTCON0 ;timer base is free run 1:1 pre/postscaler (Fcy)
;20KHz = 0xFA ;20KHz of PWM frequency Fcy=5MHz \
movlw 0x37 ;24KHZ=0xcf
movwf PTPERL ;16KHz = 0x137
;12KHz = 0x1A0
movlw 0x01
movwf PTPERH
movlw b'01001111' ;PWM0-5 enabled in independent mode
movwf PWMCON0
movlw b'00000001' ;Output overides synched wrt PWM timebase
movwf PWMCON1
movlw b'00000000' ;
movwf DTCON
movlw b'00000000' ;PWM0-5 PWM Duty cycle on overide
movwf OVDCOND
movlw b'00000000' ; All PWMs = 0 on init
movwf OVDCONS
movlw b'10110011' ;Faults on
movlw b'10000000' ;Faults on
movwf FLTCONFIG
movlw 0x00
movwf SEVTCMPL
movlw 0x00
movwf SEVTCMPH
clrf PDC0L
clrf PDC1L
clrf PDC2L
clrf PDC3L
clrf PDC0H
clrf PDC1H
clrf PDC2H
clrf PDC3H
movlw b'10000000' ;PWM timer ON
movwf PTCON1
;-----------------------------------------------------------------
;init Hall @ IC1/IC2/IC3, Timer5
bsf TRISA,2
bsf TRISA,3
bsf TRISA,4
movlw b'00000001' ;1:1
movwf T5CON
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -