📄 pid.asm
字号:
SPLK #Kp_q_,Kp_q ; Proportional gain (Q15)
SPLK #Ki_q_,Ki_q ; Integral gain (Q31-16bit) pick bit#23-#8
SPLK #Kd_q_,Kd_q ; Derivative gain (Q14)
SPLK #Kc_q_,Kc_q ; Correction gain (Q15)
SPLK Umax_q_,uq_max ; Initialize the maximum output voltage (Q15)
SPLK Umin_q_,uq_min ; Initialize the minimum output voltage (Q15)
SPLK #0,up1_q ; Initialize the error proportional (Q14)
SPLK #0,ui_hi_q ; Initialize the integral term (Q30)
SPLK #0,ui_lo_q ; Initialize the integral term (Q30)
SPLK #0,ud_hi_q ; Initialize the derivative term (Q30)
SPLK #0,ud_lo_q ; Initialize the derivative term (Q30)
RET
;------------------------------------------------------------
; Routine
;------------------------------------------------------------
PID_REG3_IQ
SETC SXM ; Allow sign extension
SETC OVM ; Set overflow protection mode
SPM 0 ; Reset Product mode
LDP #iq_ref
; e(k) = ref(k)-fdb(k) => Q14 = Q15-Q15
LACC iq_ref,15 ; ACC = ref (Q30)
SUB iq_fdb,15 ; ACC = ref-fdb (Q30)
SACH e_q ; e = ref-fdb (Q14)
; up(k) = Kp*e(k) => Q14 = Q15*Q14
LT Kp_q ; TREG = Kp (Q15)
MPY e_q ; PREG = Kp*e (Q29)
PAC ; ACC = Kp*e (Q29)
SACH up_q,1 ; up = Kp*e (Q14)
; uprsat(k) = up(k)+ui(k-1)+ud(k-1) => Q14 = Q14+Q30+Q30
LACC ui_hi_q,16 ; ACC = ui (Q30)
ADDS ui_lo_q ; ACC = ui (Q30)
ADDS ud_lo_q ; ACC = ui+ud (Q30)
ADDH ud_hi_q ; ACC = ui+ud (Q30)
ADDH up_q ; ACC = up+ui+ud (Q30)
SACH uprsat_q ; uprsat = up+ui+ud (Q14)
; Check uprsat is saturated ?
LACC uprsat_q,16 ; ACC = uprsat (Q14)
SUB uq_max,15 ; ACC = uprsat-umax (Q14)
BCND SAT_MAX_Q,GT ; Branch to SAT_MAX if uprsat > umax
LACC uprsat_q,16 ; ACC = uprsat (Q14)
SUB uq_min,15 ; ACC = uprsat-umin (Q14)
BCND SAT_MIN_Q,LT ; Branch to SAT_MIN if uprsat < umin
LACC uprsat_q,16 ; ACC = uprsat (Q30)
SACH uq_out,1 ; uout = uprsat (Q15)
B UPDATE_Q
SAT_MAX_Q
LACC uq_max ; ACC = umax (Q15)
SACL uq_out ; uout = umax (Q15)
B UPDATE_D
SAT_MIN_Q
LACC uq_min ; ACC = umin (Q15)
SACL uq_out ; uout = umin (Q15)
UPDATE_Q
LACC uq_out,15 ; ACC = uout (Q30)
SUB uprsat_q,16 ; ACC = uout-uprsat (Q30)
SACH saterr_q ; saterr = uout-uprsat (Q14)
; ui(k) = ui(k-1)+Ki*up(k)+Kc*(uout-uprsat) => Q30 = Q30+Q31*Q14+Q15*Q14
SPM 3 ; Set right shifted 6 bit
LT Ki_q ; TREG = Ki (Q31-16bit)
MPY up_q ; PREG = Ki*up (Q38)
PAC ; ACC = Ki*up (Q32)
SFR ; ACC = Ki*up (Q31)
SFR ; ACC = Ki*up (Q30)
SPM 1 ; Set left shifted 1 bit
LT Kc_q ; TREG = Kc (Q15)
MPY saterr_q ; PREG = Kc*(uout-uprsat) (Q29)
APAC ; ACC = Ki*up+Kc*(uout-uprsat) (Q30)
ADDS ui_lo_q ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
ADDH ui_hi_q ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACL ui_lo_q ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACH ui_hi_q ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
; ud(k) = Kd*up(k)-Kd*up(k-1) => Q30 = Q14*Q14-Q14*Q14
LT Kd_q ; TREG = Kd (Q14)
MPY up_q ; PREG = Kd*up (Q28)
PAC ; ACC = Kd*up (Q29)
MPY up1_q ; PREG = Kd*up1 (Q28)
SPAC ; ACC = Kd*up-Kd*up1 (Q29)
SACL ud_lo_q,1 ; ud = Kd*up-Kd*up1 (Q30)
SACH ud_hi_q,1 ; ud = Kd*up-Kd*up1 (Q30)
; Update up
LACC up_q ; ACC = up (Q14)
SACL up1_q ; up1 = up (Q14)
RET
***END Q-Axis PID Current Regulator
*************************************************************
* PID Speed Regulator
*************************************************************
;------------------------------------------------------------
; Reference/Prototype
;------------------------------------------------------------
;(To use this Module, copy this section to main system file)
; .ref PID_REG3_SPD,PID_REG3_SPD_INIT ; function call
; .ref spd_fdb,spd_ref ; Inputs
; .ref t_out ; Outputs
; .ref Kp_spd,Ki_spd,Kd_spd,Kc_spd,t_max,t_min ; Parameters
;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
;Module definitions for external reference.
.def PID_REG3_SPD,PID_REG3_SPD_INIT ; function call
.def spd_fdb,spd_ref ; Inputs
.def t_out ; Outputs
.def Kp_spd,Ki_spd,Kd_spd,Kc_spd,t_max,t_min ; Parameters
;------------------------------------------------------------
; Variable Definitions
;------------------------------------------------------------
spd_fdb .usect "pid",1 ; speed feedback
spd_ref .usect "pid",1 ; speed reference
t_out .usect "pid",1 ; control output
t_max .usect "pid",1 ; maximum output (Q15)
t_min .usect "pid",1 ; minimum output (Q15)
up_spd .usect "pid",1 ; error proportional (Q14)
up1_spd .usect "pid",1 ; error proportional at k-1 (Q14)
ui_hi_spd .usect "pid",1 ; error integral (Q30)
ui_lo_spd .usect "pid",1
ud_hi_spd .usect "pid",1 ; error derivative (Q30)
ud_lo_spd .usect "pid",1
Kp_spd .usect "pid",1 ; proportional gain
Ki_spd .usect "pid",1 ; integral gain
Kd_spd .usect "pid",1 ; derivative gain
Kc_spd .usect "pid",1 ; integral correction gain
e_spd .usect "pid",1 ; current error
uprsat_spd .usect "pid",1 ; control output before saturation
saterr_spd .usect "pid",1 ; saturation error
;------------------------------------------------------------
; Default parameters
; Parameter spreadsheet: pid.xls
;------------------------------------------------------------
Kp_spd_ .set 28312 ; Q15, proportional gain
Ki_spd_ .set 4149 ; Q31, integral gain
Kd_spd_ .set 0 ; Q14, derivative gain
Kc_spd_ .set 31858 ; Q15, saturation correction gain
Umax_spd_ .set 07FFFh ; maximum U
Umin_spd_ .set 08000h ; minimum U
;------------------------------------------------------------
; Initialization
;------------------------------------------------------------
PID_REG3_SPD_INIT
LDP #Kp_spd
SPLK #Kp_spd_,Kp_spd ; Proportional gain (Q15)
SPLK #Ki_spd_,Ki_spd ; Integral gain (Q31-16bit) pick bit#23-#8
SPLK #Kd_spd_,Kd_spd ; Derivative gain (Q14)
SPLK #Kc_spd_,Kc_spd ; Correction gain (Q15)
SPLK Umax_spd_,t_max ; Initialize the maximum output (Q15)
SPLK Umin_spd_,t_min ; Initialize the minimum output (Q15)
SPLK #0,up1_spd ; Initialize the error proportional (Q14)
SPLK #0,ui_hi_spd ; Initialize the integral term (Q30)
SPLK #0,ui_lo_spd ; Initialize the integral term (Q30)
SPLK #0,ud_hi_spd ; Initialize the derivative term (Q30)
SPLK #0,ud_lo_spd ; Initialize the derivative term (Q30)
RET
;------------------------------------------------------------
; Routine
;------------------------------------------------------------
PID_REG3_SPD
SETC SXM ; Allow sign extension
SETC OVM ; Set overflow protection mode
SPM 0 ; Reset Product mode
LDP #spd_ref
; e(k) = ref(k)-fdb(k) => Q14 = Q15-Q15
LACC spd_ref,15 ; ACC = ref (Q30)
SUB spd_fdb,15 ; ACC = ref-fdb (Q30)
SACH e_spd ; e = ref-fdb (Q14)
; up(k) = Kp*e(k) => Q14 = Q15*Q14
LT Kp_spd ; TREG = Kp (Q15)
MPY e_spd ; PREG = Kp*e (Q29)
PAC ; ACC = Kp*e (Q29)
SACH up_spd,1 ; up = Kp*e (Q14)
; uprsat(k) = up(k)+ui(k-1)+ud(k-1) => Q14 = Q14+Q30+Q30
LACC ui_hi_spd,16 ; ACC = ui (Q30)
ADDS ui_lo_spd ; ACC = ui (Q30)
ADDS ud_lo_spd ; ACC = ui+ud (Q30)
ADDH ud_hi_spd ; ACC = ui+ud (Q30)
ADDH up_spd ; ACC = up+ui+ud (Q30)
SACH uprsat_spd ; uprsat = up+ui+ud (Q14)
; Check uprsat is saturated ?
LACC uprsat_spd,16 ; ACC = uprsat (Q14)
SUB t_max,15 ; ACC = uprsat-umax (Q14)
BCND SAT_MAX_SPD,GT ; Branch to SAT_MAX if uprsat > umax
LACC uprsat_spd,16 ; ACC = uprsat (Q14)
SUB t_min,15 ; ACC = uprsat-umin (Q14)
BCND SAT_MIN_SPD,LT ; Branch to SAT_MIN if uprsat < umin
LACC uprsat_spd,16 ; ACC = uprsat (Q30)
SACH t_out,1 ; uout = uprsat (Q15)
B UPDATE_SPD
SAT_MAX_SPD
LACC t_max ; ACC = umax (Q15)
SACL t_out ; uout = umax (Q15)
B UPDATE_SPD
SAT_MIN_SPD
LACC t_min ; ACC = umin (Q15)
SACL t_out ; uout = umin (Q15)
UPDATE_SPD
LACC t_out,15 ; ACC = uout (Q30)
SUB uprsat_spd,16 ; ACC = uout-uprsat (Q30)
SACH saterr_spd ; saterr = uout-uprsat (Q14)
; ui(k) = ui(k-1)+Ki*up(k)+Kc*(uout-uprsat) => Q30 = Q30+Q31*Q14+Q15*Q14
SPM 3 ; Set right shifted 6 bit
LT Ki_spd ; TREG = Ki (Q31-16bit)
MPY up_spd ; PREG = Ki*up (Q38)
PAC ; ACC = Ki*up (Q32)
SFR ; ACC = Ki*up (Q31)
SFR ; ACC = Ki*up (Q30)
SPM 1 ; Set left shifted 1 bit
LT Kc_spd ; TREG = Kc (Q15)
MPY saterr_spd ; PREG = Kc*(uout-uprsat) (Q29)
APAC ; ACC = Ki*up+Kc*(uout-uprsat) (Q30)
ADDS ui_lo_spd ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
ADDH ui_hi_spd ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACL ui_lo_spd ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACH ui_hi_spd ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
; ud(k) = Kd*up(k)-Kd*up(k-1) => Q30 = Q14*Q14-Q14*Q14
LT Kd_spd ; TREG = Kd (Q14)
MPY up_spd ; PREG = Kd*up (Q28)
PAC ; ACC = Kd*up (Q29)
MPY up1_spd ; PREG = Kd*up1 (Q28)
SPAC ; ACC = Kd*up-Kd*up1 (Q29)
SACL ud_lo_spd,1 ; ud = Kd*up-Kd*up1 (Q30)
SACH ud_hi_spd,1 ; ud = Kd*up-Kd*up1 (Q30)
; Update up
LACC up_spd ; ACC = up (Q14)
SACL up1_spd ; up1 = up (Q14)
RET
***END PID Speed Regulator
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -