?? pid.asm
字號:
;=======================================================================
; File Name: pid.asm
;
; Module Name: PID_REG3_ID, PID_REG3_IQ, PID_REG3_SPD
;
; Initialization Routine: PID_REG3_ID_INIT, PID_REG3_IQ_INIT,
; PID_REG3_SPD_INIT
;
; Originator: Digital Control Systems Group
; Texas Instruments
;
; Description: Three PID regulators with integral correction for
; i) Id regulator
; ii) Iq regulator
; iii) Speed regulator
;
; |~~~~~~~~~~~~|
; fdb o------>| |
; ref o------>| |
; | pid_reg3 |----->o out
; Kp o------>| |
; Ki o------>| |
; Kd o------>| |
; Kc o------>|____________|
;
; Target dependency: C2xx core only
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 11-26-2000 Release Rev 1.00
;===========================================================================
*************************************************************
* D-Axis PID Current Regulator
*************************************************************
;------------------------------------------------------------
; Reference/Prototype
;------------------------------------------------------------
;(To use this Module, copy this section to main system file)
; .ref PID_REG3_ID,PID_REG3_ID_INIT ; function call
; .ref id_fdb,id_ref ; Inputs
; .ref ud_out ; Outputs
; .ref Kp_d,Ki_d,Kd_d,Kc_d,ud_max,ud_min ; Parameters
;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
;Module definitions for external reference.
.def PID_REG3_ID,PID_REG3_ID_INIT ; function call
.def id_fdb,id_ref ; Inputs
.def ud_out ; Outputs
.def Kp_d,Ki_d,Kd_d,Kc_d,ud_max,ud_min ; Parameters
;------------------------------------------------------------
; Variable Definitions
;------------------------------------------------------------
id_fdb .usect "pid",1 ; current feedback
id_ref .usect "pid",1 ; current reference
ud_out .usect "pid",1 ; control voltage output
ud_max .usect "pid",1 ; maximum output voltage (Q15)
ud_min .usect "pid",1 ; minimum output voltage (Q15)
up_d .usect "pid",1 ; error proportional (Q14)
up1_d .usect "pid",1 ; error proportional at k-1 (Q14)
ui_hi_d .usect "pid",1 ; error integral (Q30)
ui_lo_d .usect "pid",1
ud_hi_d .usect "pid",1 ; error derivative (Q30)
ud_lo_d .usect "pid",1
Kp_d .usect "pid",1 ; proportional gain
Ki_d .usect "pid",1 ; integral gain
Kd_d .usect "pid",1 ; derivative gain
Kc_d .usect "pid",1 ; integral correction gain
e_d .usect "pid",1 ; current error
uprsat_d .usect "pid",1 ; control voltage before saturation
saterr_d .usect "pid",1 ; saturation error
;------------------------------------------------------------
; Default parameters
; Parameter spreadsheet: pid.xls
;------------------------------------------------------------
Kp_d_ .set 21178 ; Q15, proportional gain
Ki_d_ .set 4194 ; Q31, integral gain
Kd_d_ .set 0 ; Q14, derivative gain
Kc_d_ .set 20281 ; Q15, saturation correction gain
Umax_d_ .set 04000h ; maximum U
Umin_d_ .set 0C000h ; minimum U
;------------------------------------------------------------
; Initialization
;------------------------------------------------------------
PID_REG3_ID_INIT
LDP #Kp_d
SPLK #Kp_d_,Kp_d ; Proportional gain (Q15)
SPLK #Ki_d_,Ki_d ; Integral gain (Q31-16bit) pick bit#23-#8
SPLK #Kd_d_,Kd_d ; Derivative gain (Q14)
SPLK #Kc_d_,Kc_d ; Correction gain (Q15)
SPLK Umax_d_,ud_max ; Initialize the maximum output voltage (Q15)
SPLK Umin_d_,ud_min ; Initialize the minimum output voltage (Q15)
SPLK #0,up1_d ; Initialize the error proportional (Q14)
SPLK #0,ui_hi_d ; Initialize the integral term (Q30)
SPLK #0,ui_lo_d ; Initialize the integral term (Q30)
SPLK #0,ud_hi_d ; Initialize the derivative term (Q30)
SPLK #0,ud_lo_d ; Initialize the derivative term (Q30)
RET
;------------------------------------------------------------
; Routine
;------------------------------------------------------------
PID_REG3_ID
SETC SXM ; Allow sign extension
SETC OVM ; Set overflow protection mode
SPM 0 ; Reset Product mode
LDP #id_ref
; e(k) = ref(k)-fdb(k) => Q14 = Q15-Q15
LACC id_ref,15 ; ACC = ref (Q30)
SUB id_fdb,15 ; ACC = ref-fdb (Q30)
SACH e_d ; e = ref-fdb (Q14)
; up(k) = Kp*e(k) => Q14 = Q15*Q14
LT Kp_d ; TREG = Kp (Q15)
MPY e_d ; PREG = Kp*e (Q29)
PAC ; ACC = Kp*e (Q29)
SACH up_d,1 ; up = Kp*e (Q14)
; uprsat(k) = up(k)+ui(k-1)+ud(k-1) => Q14 = Q14+Q30+Q30
LACC ui_hi_d,16 ; ACC = ui (Q30)
ADDS ui_lo_d ; ACC = ui (Q30)
ADDS ud_lo_d ; ACC = ui+ud (Q30)
ADDH ud_hi_d ; ACC = ui+ud (Q30)
ADDH up_d ; ACC = up+ui+ud (Q30)
SACH uprsat_d ; uprsat = up+ui+ud (Q14)
; Check uprsat is saturated ?
LACC uprsat_d,16 ; ACC = uprsat (Q14)
SUB ud_max,15 ; ACC = uprsat-umax (Q14)
BCND SAT_MAX_D,GT ; Branch to SAT_MAX if uprsat > umax
LACC uprsat_d,16 ; ACC = uprsat (Q14)
SUB ud_min,15 ; ACC = uprsat-umin (Q14)
BCND SAT_MIN_D,LT ; Branch to SAT_MIN if uprsat < umin
LACC uprsat_d,16 ; ACC = uprsat (Q30)
SACH ud_out,1 ; uout = uprsat (Q15)
B UPDATE_D
SAT_MAX_D
LACC ud_max ; ACC = umax (Q15)
SACL ud_out ; uout = umax (Q15)
B UPDATE_D
SAT_MIN_D
LACC ud_min ; ACC = umin (Q15)
SACL ud_out ; uout = umin (Q15)
UPDATE_D
LACC ud_out,15 ; ACC = uout (Q30)
SUB uprsat_d,16 ; ACC = uout-uprsat (Q30)
SACH saterr_d ; 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_d ; TREG = Ki (Q31-16bit)
MPY up_d ; 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_d ; TREG = Kc (Q15)
MPY saterr_d ; PREG = Kc*(uout-uprsat) (Q29)
APAC ; ACC = Ki*up+Kc*(uout-uprsat) (Q30)
ADDS ui_lo_d ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
ADDH ui_hi_d ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACL ui_lo_d ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACH ui_hi_d ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
; ud(k) = Kd*up(k)-Kd*up(k-1) => Q30 = Q14*Q14-Q14*Q14
LT Kd_d ; TREG = Kd (Q14)
MPY up_d ; PREG = Kd*up (Q28)
PAC ; ACC = Kd*up (Q29)
MPY up1_d ; PREG = Kd*up1 (Q28)
SPAC ; ACC = Kd*up-Kd*up1 (Q29)
SACL ud_lo_d,1 ; ud = Kd*up-Kd*up1 (Q30)
SACH ud_hi_d,1 ; ud = Kd*up-Kd*up1 (Q30)
; Update up
LACC up_d ; ACC = up (Q14)
SACL up1_d ; up1 = up (Q14)
RET
***END D-Axis PID Current Regulator
*************************************************************
* Q-Axis PID Current Regulator
*************************************************************
;------------------------------------------------------------
; Reference/Prototype
;------------------------------------------------------------
;(To use this Module, copy this section to main system file)
; .ref PID_REG3_IQ,PID_REG3_IQ_INIT ; function call
; .ref iq_fdb,iq_ref ; Inputs
; .ref uq_out ; Outputs
; .ref Kp_q,Ki_q,Kd_q,Kc_q,uq_max,uq_min ; Parameters
;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
;Module definitions for external reference.
.def PID_REG3_IQ,PID_REG3_IQ_INIT ; function call
.def iq_fdb,iq_ref ; Inputs
.def uq_out ; Outputs
.def Kp_q,Ki_q,Kd_q,Kc_q,uq_max,uq_min ; Parameters
;------------------------------------------------------------
; Variable Definitions
;------------------------------------------------------------
iq_fdb .usect "pid",1 ; current feedback
iq_ref .usect "pid",1 ; current reference
uq_out .usect "pid",1 ; control voltage output
uq_max .usect "pid",1 ; maximum output voltage (Q15)
uq_min .usect "pid",1 ; minimum output voltage (Q15)
up_q .usect "pid",1 ; error proportional (Q14)
up1_q .usect "pid",1 ; error proportional at k-1 (Q14)
ui_hi_q .usect "pid",1 ; error integral (Q30)
ui_lo_q .usect "pid",1
ud_hi_q .usect "pid",1 ; error derivative (Q30)
ud_lo_q .usect "pid",1
Kp_q .usect "pid",1 ; proportional gain
Ki_q .usect "pid",1 ; integral gain
Kd_q .usect "pid",1 ; derivative gain
Kc_q .usect "pid",1 ; integral correction gain
e_q .usect "pid",1 ; current error
uprsat_q .usect "pid",1 ; control voltage before saturation
saterr_q .usect "pid",1 ; saturation error
;------------------------------------------------------------
; Default parameters
; Parameter spreadsheet: pid.xls
;------------------------------------------------------------
Kp_q_ .set 21178 ; Q15, proportional gain
Ki_q_ .set 4194 ; Q31, integral gain
Kd_q_ .set 0 ; Q14, derivative gain
Kc_q_ .set 20281 ; Q15, saturation correction gain
Umax_q_ .set 06700h ; maximum U
Umin_q_ .set 09900h ; minimum U
;------------------------------------------------------------
; Initialization
;------------------------------------------------------------
PID_REG3_IQ_INIT
LDP #Kp_q
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -