📄 c7_2_2.asm
字号:
.title "Fuzzy_PI.asm"
.include "lf2407_regs.h"
.include "Fuzzy_PI_vec.asm"
.def _cy_begin
Kp_init .set 100h
Ki_high_init .set 00h
Ki_low_init .set 1000h
Kd_init .set 05h
PID_ref_init .set 500
PID_output_MAX .set 07FFFh
PID_output_MIN .set 0h
.data
ADRESULT .word 00h
PID_input .word 00h
PID_output .word 00h
PID_output1 .word 00h
PID_reference .word 00h
PID_e0 .word 00h
PID_e1 .word 00h
PID_delta_e .word 00h
q1 .word 00h
q2 .word 00h
k1 .word 00h
k2 .word 00h
Fuzzy_PID_e
Fuzzy_PID_delta_e
Fuzzy_KP
Fuzzy_KI
Kp .word 00h
Ki_high .word 00h
Ki_low .word 00h
Kd .word 00h
A_coeff_high .word 00h
A_coeff_low .word 00h
B_coeff .word 00h
tmp1_high .word 00h
tmp1_low .word 00h
tmp2_high .word 00h
tmp2_low .word 00h
tmp3 .word 00h
e0_sign .word 00h
abs_e0 .word 00h
.text
_cy_begin:
NOP
CALL system_init
CALL PID_init
CALL cy_AD_init
cy_LOOP:
NOP
NOP
B cy_LOOP
system_init:
SETC INTM
CLRC OVM
CLRC SXM
CLRC CNF
LDP #DP_PF1
SPLK #081FEh,SCSR1
SPLK #0E8h,WDCR
LDP #0
SPLK #0001h,IMR
SPLK #0FFFFh,IFR
RET
cy_AD_init:
LDP #DP_EVB
SPLK #0000h, T4CNT
SPLK #0176h, T4PR
SPLK #0400h, GPTCONB
SPLK #0170Ch, T4CON
SPLK #0FFFFh, EVBIFRA
SPLK #0FFFFh, EVBIFRB
SPLK #0FFFFh, EVBIFRC
SPLK #00000h, EVBIMRA
SPLK #00000h, EVBIMRB
SPLK #00000h, EVBIMRC
LDP #DP_PF2
SPLK #0000h, ADCCTRL1
SPLK #8404h, ADCCTRL2
SPLK #0001h, MAXCONV
SPLK #0000h, CHSELSEQ1
SPLK #0000h, CHSELSEQ2
SPLK #0000h, CHSELSEQ3
SPLK #0000h, CHSELSEQ4
CALL AD_Start
LDP #4
SPLK #0000h, ADRESULT
RET
AD_Start:
LDP #DP_EVB
LACL T4CON
OR #0040h
SACL T4CON
RET
PI_Control:
SETC SXM
SETC OVM
SPM #0
LDP #4
LACL ADRESULT
SACL PID_input
CALL Fuzzy_PI_table
LACC Ki_high, 16
ADDS Ki_low
ADD Kp, 16
ADD Kd, 16
SACH A_coeff_high
SACL A_coeff_low
LACC Kd, 16
SFL
ADD Kp, 16
SACH B_coeff
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
LACC PID_reference
SUB PID_input
SACL PID_e0
LT Kd
MPY PID_e2
PAC
LT B_coeff
MPY PID_e1
SPAC
SACH tmp1_high, 1
SACL tmp1_low, 1
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
LACC PID_e0
SACL e0_sign
ABS
SACL abs_e0
LT abs_e0
MPYU A_coeff_low
SPH tmp2_low
MPYU A_coeff_high
PAC
ADDS tmp2_low
SACH tmp2_high, 1
SACL tmp2_low, 1
LACC e0_sign
BCND cy_DONE, GT
LACC tmp2_high, 16
ADDS tmp2_low
NEG
SACH tmp2_high
SACL tmp2_low
cy_DONE:
LACC tmp1_high, 16
ADDS tmp1_low
ADDS tmp2_low
ADD tmp2_high, 16
SACH tmp3
LACC PID_output1, 16
ADD tmp3, 16
SACH PID_output
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
LACC PID_output
SUB #PID_output_MAX
BCND greater_MAX, GT
LACC PID_output
SUB #PID_output_MIN
BCND less_MIN, LT
B PID_end
greater_MAX:
SPLK #PID_output_MAX, PID_output
B PID_end
less_MIN:
SPLK #PID_output_MIN, PID_output
PID_end:
LDP #4
LACC PID_e1
SACL PID_e2
LACC PID_e0
SACL PID_e1
LACC PID_output
SACL PID_output1
CLRC SXM
RET
PID_init:
LDP #4
SPLK #Kp_init, Kp
SPLK #Ki_high_init, Ki_high
SPLK #Ki_low_init, Ki_low
SPLK #Kd_init, Kd
SPLK #PID_ref_init, PID_reference
SPLK #0, PID_e2
SPLK #0, PID_e1
SPLK #0, PID_e0
SPLK #0, PID_output1
SPLK #0, PID_output
RET
Fuzzy_PI_table:
LACC PID_reference
SUB PID_input
SACL PID_e0
SUB PID_e1
SACL PID_delta_e
LT q1
MPY PID_e0
PAC
SACL Fuzzy_PID_e
LT q2
MPY PID_delta_e
PAC
SACL Fuzzy_PID_delta_e
LACL Fuzzy_PID_e
SUB #6
BCND Normal_Fuzzy_PI_1, LT
SPLK #6, Fuzzy_PID_e
Normal_Fuzzy_PI_1:
LACL Fuzzy_PID_delta_e
SUB #6
BCND Normal_Fuzzy_PI_2, LT
SPLK #6, Fuzzy_PID_delta_e
Normal_Fuzzy_PI_2:
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
LACL Fuzzy_PID_e
SUB #5
BCND Fuzzy_PID_e_5, EQ
B Fuzzy_PID_e_not5
Fuzzy_PID_e_5:
LACL Fuzzy_PID_delta_e
SUB #1
BCND Fuzzy_PID_delta_e_1, EQ
B Fuzzy_PID_e_not5
Fuzzy_PID_delta_e_5:
SPLK #5, Fuzzy_KP
SPLK #0, Fuzzy_KI
B Fuzzy_PI_end
Fuzzy_PID_e_not5:
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
LACL Fuzzy_PID_e
SUB #4
BCND Fuzzy_PID_e_4, EQ
B Fuzzy_PID_e_not4
Fuzzy_PID_e_4:
LACL Fuzzy_PID_delta_e
SUB #2
BCND Fuzzy_PID_delta_e_2, EQ
B Fuzzy_PID_e_not4
Fuzzy_PID_delta_e_5:
SPLK #4, Fuzzy_KP
SPLK #1, Fuzzy_KI
B Fuzzy_PI_end
Fuzzy_PID_e_not4:
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
LACL Fuzzy_PID_e
SUB #3
BCND Fuzzy_PID_e_3, EQ
B Fuzzy_PID_e_not3
Fuzzy_PID_e_3:
LACL Fuzzy_PID_delta_e
SUB #3
BCND Fuzzy_PID_delta_e_3, EQ
B Fuzzy_PID_e_not3
Fuzzy_PID_delta_e_3:
SPLK #3, Fuzzy_KP
SPLK #2, Fuzzy_KI
B Fuzzy_PI_end
Fuzzy_PID_e_not3:
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
Fuzzy_PI_end:
LT k1
MPY Fuzzy_KP
PAC
SACL Kp
LT k2
MPY Fuzzy_KI
PAC
SACL Ki
RET
GISR1:
;保护现场
LDP #DP_PF1
LACC PIVR,1
ADD #PVECTORS
BACC
ADCINT_ISR:
CLRC SXM
LDP #4
LAR AR2, #RESULT0
MAR *, AR2
LACC *, 10
SACH ADRESULT
CALL PI_Control
CLRC INTM
RET
PHANTOM:
LDP #DP_PF1
SPLK #05555h, WDKEY
SPLK #0AAAAh, WDKEY
RET
.end
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -