📄 pid_main.asm
字号:
movff deriv0,BARGB0 ;YES, AARGB0:AARGB2 has result of Prop + Integ
movff deriv1,BARGB1 ;load derivative term
movff deriv2,BARGB2
movff pid_stat1,temp_reg ;pid_stat1 ---> temp_reg
movlw b'11000000' ;prepare for sign check of bits 7 & 6
andwf temp_reg,f,1
movf temp_reg,w,1 ;check error sign & a_error sign bits
sublw 0x00
btfsc STATUS,Z
bra add_neg_d ;bits 7 & 6 (00) are NEGATIVE, add them
bra other_combo_d ;bits 7 & 6 not equal to 00
add_neg_d
call _24_BitAdd ;add negative sign values
bra scale_down ;scale result
other_combo_d
movf temp_reg,w,1
sublw 0xC0
btfsc STATUS,Z
bra add_pos_d ;bits 7 & 6 (11) are POSITIVE, add them
bra find_mag_sub_d ;bits 7 & 6 (xx) are different signs , subtract them
add_pos_d
call _24_BitAdd ;add positive sign values
bra scale_down ;scale result
find_mag_sub_d
call mag_and_sub ;subtract unlike sign numbers
btfss pid_stat1,mag,1 ;which is greater in magnitude ?
bra deriv_mag ;BARGB is greater in magnitude
bra scale_down ;derivative term < part pid term, leave pid_sign as is
deriv_mag ;derivative term > part pid term
bcf pid_stat1,pid_sign,1 ;PID result is negative
btfsc pid_stat1,d_err_sign,1
bsf pid_stat1,pid_sign,1 ;PID result is positive
scale_down
clrf BARGB0,1 ;(Prop + Integ + Deriv) / 16 = FINAL PID RESULT to plant
movlw 0x10
movwf BARGB1,1
call FXD2416U
movff AARGB2,pid_out2 ;final result ---> pid_out2
movff AARGB1,pid_out1 ;final result ---> pid_out1
movff AARGB0,pid_out0 ;final result ---> pid_out0
#ifdef pid_100 ;Final result needs to be scaled down to 0 - 100%
movlw 0x01 ;% ratio for propotional only
movwf BARGB0,1
movlw 0x90
movwf BARGB1,1
btfss pid_stat2,integ_go,1 ;if integral is included then change % conversion
bra conv_percent ;no do proportional only
bcf pid_stat2,integ_go,1 ;clear integral go bit for next operation
movlw 0x03 ;% ratio for propotional & integral only
movwf BARGB0,1
movlw 0x20
movwf BARGB1,1
btfss pid_stat2,deriv_go,1 ;if derivative is included then change % conversion
bra conv_percent ;no do proportional & integral only
bcf pid_stat2,deriv_go,1 ;clear derivative go bit for next operation
movlw 0x06 ;% ratio for propotional & integral & derivative
movwf BARGB0,1
movlw 0x40
movwf BARGB1,1
conv_percent
call FXD2416U ;pid_out0:pid_out2 / % ratio = 0 - 100% value
movf AARGB2,W,1 ;AARGB2 --> percent_out
movwf percent_out,1 ;error has been scaled down and is now available in a 0 -100% range
#endif
movff error0,p_error0 ;maintain previous error for derivative term
movff error1,p_error1 ;maintain previous error for derivative term
bcf pid_stat1,p_err_sign,1 ;make p_error negative
btfsc pid_stat1,err_sign,1 ;make p_error the same sign as error
bsf pid_stat1,p_err_sign,1 ;make p_error positive
bsf PIE1,TMR1IE ;re-enable Timer 1 interrupt
return ;return to mainline code
;--------- Find Accumulative Error for Integral, Zero? Limits? --------------------
get_a_error
movff a_error0,BARGB0 ;load error & a_error
movff a_error1,BARGB1
movff a_error2,BARGB2
movff error0,AARGB1
movff error1,AARGB2
call spec_sign ;call routine for add/sub sign numbers
btfss pid_stat1,mag,1 ;which is greater in magnitude ?
bra a_err_zero ;bargb, keep sign as is or both are same sign
bcf pid_stat1,a_err_sign,1 ;aargb, make sign same as error, a_error is negative
btfsc pid_stat1,err_sign,1
bsf pid_stat1,a_err_sign,1 ;a_error is positive
a_err_zero
bcf pid_stat1,a_err_z,1 ;clear a_error zero flag
movlw 0
cpfseq AARGB0,1 ;is byte 0 = 00
bra chk_a_err_limit ;NO, done checking
cpfseq AARGB1 ;is byte 1 = 00
bra chk_a_err_limit ;NO, done checking
cpfseq AARGB2,1 ;is byte 2 = 00
bra chk_a_err_limit ;NO, done checking
bsf pid_stat1,a_err_z,1 ;YES, set zero flag
movff AARGB0,a_error0 ;store the a_error
movff AARGB1,a_error1
movff AARGB2,a_error2
return ;a_error = 00, return
chk_a_err_limit
movff AARGB0,a_error0 ;store the a_error
movff AARGB1,a_error1
movff AARGB2,a_error2
movlw 0 ;a_error reached limits?
cpfseq a_error0,1 ;Is a_error0 > 0 ??, if yes limit has been exceeded
bra restore_limit ;YES, restore limit value
cpfseq a_error1,1 ;Is a_error1 = 0 ??, if yes, limit not exceeded
bra chk_a_error1 ;NO
return ;YES
chk_a_error1
movlw a_err_1_lim
cpfsgt a_error1,1 ;Is a_error1 > a_err_1_lim??
return ;NO
bra restore_limit ;YES, restore limit value
chk_a_error2
movlw a_err_2_lim
cpfsgt a_error2,1 ;Is a_error2 > a_err_2_lim ??
return ;NO, return to mainline code
restore_limit
clrf a_error0,1 ;YES, a_error limit has been exceeded
movlw 0x0F
movwf a_error1,1
movlw 0xA0
movwf a_error2,1
return ;return to mainline code
;-------------- Find Delta Error for Derivative, Zero? --------------------
get_delta_error
clrf AARGB0,1 ;load error and p_error
movff error0,AARGB1
movff error1,AARGB2
clrf BARGB0,1
movff p_error0,BARGB1
movff p_error1,BARGB2
movf pid_stat1,w,1 ;pid_stat1 ---> temp_reg
movwf temp_reg,1 ;prepare for sign check of bits 4 & 2
movlw b'00010100'
andwf temp_reg,f,1
movf temp_reg,w,1 ;check error sign & a_error sign bits
sublw 0x00
btfsc STATUS,Z
bra p_err_neg ;bits 4 & 2 (00) are NEGATIVE,
bra other_combo2 ;bits 4 & 2 not equal to 00
p_err_neg
call mag_and_sub
bcf pid_stat1,d_err_sign,1 ;d_error is negative
btfsc pid_stat1,p_err_sign,1 ;make d_error sign same as p_error sign
bsf pid_stat1,d_err_sign,1 ;d_error is positive
bra d_error_zero_chk ;check if d_error = 0
other_combo2
movf temp_reg,w,1
sublw 0x14
btfsc STATUS,Z
bra p_err_pos ;bits 4 & 2 (11) are POSITIVE
bra p_err_add ;bits 4 & 2 (xx) are different signs
p_err_pos
call mag_and_sub
bcf pid_stat1,d_err_sign,1 ;d_error is negative
btfsc pid_stat1,p_err_sign,1 ;make d_error sign same as p_error sign
bsf pid_stat1,d_err_sign,1 ;d_error is positive
bra d_error_zero_chk ;check if d_error = 0
p_err_add
call _24_BitAdd ;errors are different sign
bcf pid_stat1,d_err_sign,1 ;d_error is negative
btfsc pid_stat1,err_sign,1 ;make d_error sign same as error sign
bsf pid_stat1,d_err_sign,1 ;d_error is positive
d_error_zero_chk
movff AARGB1,d_error0
movff AARGB2,d_error1
bcf pid_stat2,d_err_z,1
movlw 0
cpfseq d_error0,1 ;is d_error0 = 00
return ;NO, done checking
cpfseq d_error1,1 ;YES, is d_error1 = 00
return ;NO, done checking
bsf pid_stat2,d_err_z,1 ;set delta error zero bit
return ;YES, return to ISR
;---------------------- Special Sign Routine ---------------------------------
spec_sign
movff pid_stat1,temp_reg ;pid_stat1 ---> temp_reg
movlw b'00001100' ;prepare for sign check of bits 3 & 2
andwf temp_reg,f,1
movf temp_reg,w,1 ;check error sign & a_error sign bits
sublw 0x00
btfsc STATUS,Z
bra add_neg ;bits 3 & 2 (00) are NEGATIVE, add them
bra other_combo ;bits 3 & 2 not equal to 00
add_neg
call _24_BitAdd ;add negative sign values
return
other_combo
movf temp_reg,w,1
sublw 0x0C
btfsc STATUS,Z
bra add_pos ;bits 3 & 2 (11) are POSITIVE, add them
bra find_mag_sub ;bits 3 & 2 (xx) are different signs , subtract them
add_pos
call _24_BitAdd ;add positive sign values
return
find_mag_sub
call mag_and_sub ;subtract unlike sign numbers
return
;------------------- Find Magnitude and Subtract -------------------------
mag_and_sub
movf BARGB0,w,1
subwf AARGB0,w,1 ;AARGB0 - BARGB0 --> W
btfsc STATUS,Z ;= zero ?
bra check_1 ;YES
btfsc STATUS,C ;borrow ?
bra aargb_big ;AARGB0 > BARGB0, no borrow
bra bargb_big ;BARGB0 > AARGB0, borrow
check_1
movf BARGB1,w,1
subwf AARGB1,w,1 ;AARGB1 - BARGB1 --> W
btfsc STATUS,Z ;= zero ?
bra check_2 ;YES
btfsc STATUS,C ;borrow ?
bra aargb_big ;AARGB1 > BARGB1, no borrow
bra bargb_big ;BARGB1 > AARGB1, borrow
check_2
movf BARGB2,w,1 ;AARGB2 - BARGB2 --> W
subwf AARGB2,w,1
btfsc STATUS,C ;borrow ?
bra aargb_big ;AARGB2 > BARGB2, no borrow
bra bargb_big ;BARGB2 > AARGB2, borrow
aargb_big
call _24_bit_sub
bsf pid_stat1,mag,1 ;AARGB is greater in magnitude
return
bargb_big
movf BARGB1,W,1 ;swap AARGB1 with BARGB1
movff AARGB1,temp_reg
movwf AARGB1,1
movff temp_reg,BARGB1
movf BARGB2,W,1 ;swap AARGB2 with BARGB2
movff AARGB2,temp_reg
movwf AARGB2,1
movff temp_reg,BARGB2
call _24_bit_sub ;BARGB > AARGB
bcf pid_stat1,mag,1 ;BARGB is greater in magnitude
return
;------------------- High Interrupt Routine -------------------------
;highInt
; btfss PIR1,TMR1IF ;has T1 overflowed?
; retfie ;NO, exit ISR
PID_INT
GLOBAL PID_INT
bcf PIR1,TMR1IF ;YES, clear T1 interrupt flag
movlw timer1_hi ;reload T1 registers with 1ms count
movwf TMR1H
movlw timer1_lo
movwf TMR1L
bsf pid_stat2,timer_expire,1
call PID_INT_CALC
retfie FAST ;YES
PID_INT_CALC
bcf pid_stat2,timer_expire,1
btfsc pid_stat1,err_z,1 ;Is error = 00 ?
return
movwf T_WREG,1
movff STATUS,T_STATUS
movff BSR,T_BSR ;if needed
movff AARGB0,T_AARGB0 ;NO, context save Math varialbes
movff AARGB1,T_AARGB1
movff AARGB2,T_AARGB2
movff AARGB3,T_AARGB3
movff BARGB0,T_BARGB0
movff BARGB1,T_BARGB1
movff BARGB2,T_BARGB2
movff BARGB3,T_BARGB3
movff REMB0,T_REMBO
movff REMB1,T_REMB1
movff TEMP,T_TEMP
movff TEMPB0,T_TEMPB0
movff TEMPB1,T_TEMPB1
movff TEMPB2,T_TEMPB2
movff TEMPB3,T_TEMPB3
movff ZARGB0,T_ZARGB0
movff ZARGB1,T_ZARGB1
movff ZARGB2,T_ZARGB2
movff CARGB2,T_CARGB2
movff LOOPCOUNT,T_LOOPCOUNT
movff temp_reg,t_temp_reg
btfsc pid_stat1,a_err_z,1 ;Is a_error = 0
bra derivative_ready? ;YES, check if derivative is ready
bsf pid_stat2,integ_go,1 ;NO, set intergral go bit for PID calculation
call start_integral ;find integral
derivative_ready?
decfsz deriv_count,f,1 ;is it time for derivative term ?
bra skip_deriv ;NO, finish ISR
call start_deriv ;YES, find derivative
;---Get ready for Next Derivative Term
movlw deriv_cnt
movwf deriv_count,1 ;derivative action = TMR1H:TMR1L * deriv_count
skip_deriv
; movlw timer1_hi ;reload T1 registers with 1ms count
; movwf TMR1H
; movlw timer1_lo
; movwf TMR1L
movf T_WREG,w,1
movff T_STATUS,STATUS
movff T_BSR,BSR ;if needed
movff T_AARGB0,AARGB0 ;restore Math variables
movff T_AARGB1,AARGB1
movff T_AARGB2,AARGB2
movff T_AARGB3,AARGB3
movff T_BARGB0,BARGB0
movff T_BARGB1,BARGB1
movff T_BARGB2,BARGB2
movff T_BARGB3,BARGB3
movff T_REMBO,REMB0
movff T_REMB1,REMB1
movff T_TEMP,TEMP
movff T_TEMPB0,TEMPB0
movff T_TEMPB1,TEMPB1
movff T_TEMPB2,TEMPB2
movff T_TEMPB3,TEMPB3
movff T_ZARGB0,ZARGB0
movff T_ZARGB1,ZARGB1
movff T_ZARGB2,ZARGB2
movff T_CARGB2,CARGB2
movff T_LOOPCOUNT,LOOPCOUNT
movff t_temp_reg,temp_reg
; RETFIE ;return from interrupt
return
END ;directive 'end of program'
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -