📄 imvc07_a.asm
字号:
;*****************************************************************************
; File Name : IMVC07_a.asm
; Project : IMVC position control on ACPM750 AC Power Module Kit with
; TMS320LF2407 DSP controller
;=============================================================================
; Target Sys : MSK2407 DSP board + ACPM750 v3.2 power module
; Description : Implementation of controllers, coordinates transformation,
; measurement, PWM command a.o. functions
; Originator/s: Technosoft Ltd.
; Status : OK
;=============================================================================
; Copyright ?2000 Technosoft
;=============================================================================
;=============================================================================
; Include files
;=============================================================================
.include "IMVC07_a.h"
.include "vect240x.h"
;=============================================================================
; Code section
;=============================================================================
.text
;=============================================================================
; Routine Name: _init_pi_reg_id
; -----------------------------
; Description: Setup of id current, PI controller variables
; -----------------------------
; Calling Convention: extern void init_pi_reg_id();
;=============================================================================
.global _init_pi_reg_id
;=============================================================================
_init_pi_reg_id:
LDP #_i_d
LACC #0
SACL _id_1
SACL _id_ref_1
SACL _i_d
SACL _i_d_ref
SACL _e_id_1
SACL _I_id_1_high
SACL _I_id_1_low
SACL _u_d_ref
SACL _ud_ref_low
SACL _e_id_1
SACL _e_id_1
RET
;=============================================================================
; Routine Name: _pi_reg_id
; ------------------------
; Description: Implements id current PI controller
;
; OVM=1
; dyi=sat(yi-yim1)
; dyri=sat(yri-yrim1)
; ei=sat(eim1+dyri-dyi)
; dyri=ei-eim1+dyi
; Pi=satsfl(Kps*dyri-Kps*dyi)
; uim1=ui=sat(Pi+Iim1+uim1)
; Iim1=satsfl(Kis*ei)
; yrim1=yrim1+dyri
; yim1=yi
; eim1=ei
; OVM=0
; ------------------------
; Calling Convention: extern void _pi_reg_id();
;==============================================================================
; +--------------------------------------------------------------------------+
; | Input global variables: |
; +--------------------------------------------------------------------------+
; | _i_d_ref | reference value of i_d current at step i (yri) |
; | _i_d | feedback value of i_d current at step i (yi) |
; | _Kps_id | scaled Kp coeff: Kps = Kp / (2^scalKp) so that |
; | | -1 < Kps < +1 |
; | _sf_P_id | sf_P_id = scalKp |
; | _Kis_id | scaled Ki coeff: Kis = Ki / (2^scalKi) so that |
; | | -1 < Kis < +1 |
; | _sf_I_id | sf_I_id = scalKi |
; | _i_d_ref_1 | reference value of i_d current at step i-1 (yrim1) |
; | _i_d_1 | feedback value of i_d current at step i-1 (yim1) |
; | _e_id_1 | error at step i-1 |
; | _I_id_1_high | integral term at step i-1, (16 MSB) |
; | _I_id_1_low | integral term at step i-1, (16 LSB) |
; +--------------------------------------------------------------------------+
; | Input local variables: |
; +--------------------------------------------------------------------------+
; | dif_id_ref | difference between actual and old reference |
; | dif_id | difference between actual and old feedback |
; | e_id | error at step i |
; +--------------------------------------------------------------------------
; | Output global variables: |
; +--------------------------------------------------------------------------+
; | _u_d_ref | output of the regulator (16 MSB) |
; | _ud_ref_low | output of the regulator (16 LSB) |
; +--------------------------------------------------------------------------+
;-----------------------------------------------------------------------------
.global _pi_reg_id
;=============================================================================
_pi_reg_id:
SETC SXM
LDP #_i_d
SETC OVM ; set overflow protection mode
LACC _i_d,16
SUB _id_1,16
SACH dif_id ; dyi=sat(yi-yim1)
LACC _i_d_ref,16
SUB _id_ref_1,16
SACH dif_id_ref ; dyri=sat(yri-yrim1)
SUB dif_id,16 ; ACC=dyri-dyi
ADD _e_id_1,16
SACH e_id ; ei=sat(eim1+dyri-dyi)
SUB _e_id_1,16
ADD dif_id,16
SACH dif_id_ref ; dyri=ei-eim1+dyi
LT _Kps_id
MPY dif_id_ref ; Kps*dyri
PAC
MPY dif_id ; Kps*dyi
SPAC ; Kps*dyri-Kps*dyi
SATSFL _sf_P_id ; ACC=Pi=satsfl(Kps*dyri-Kps*dyi) in Q31 format
CLRC SXM
ADD _I_id_1_low
ADD _ud_ref_low
SETC SXM
ADD _I_id_1_high,16
ADD _u_d_ref,16
sat_reg_out C_SAT_U_D_REF ; macro that saturates regulator's output
SACH _u_d_ref ; uim1=ui=sat(Pi+Iim1+uim1)
SACL _ud_ref_low
LT _Kis_id
MPY e_id
PAC ; Kis*ei
SATSFL _sf_I_id ; ACC=Iim1=satsfl(Kis*ei)
SACH _I_id_1_high ; Iim1=satsfl(Kis*ei)
SACL _I_id_1_low
LACC _id_ref_1,16
ADD dif_id_ref,16
SACH _id_ref_1 ; yrim1=yrim1+dyri
LACC _i_d
SACL _id_1 ; yim1=yi
LACC e_id
SACL _e_id_1 ; eim1=ei
CLRC OVM ; disable overflow protection mode
RET
;=============================================================================
; Routine Name: _init_pi_reg_iq
; -----------------------------
; Description: Setup of iq current, PI controller variables
; -----------------------------
; Calling Convention: extern void _init_pi_reg_iq();
;=============================================================================
.global _init_pi_reg_iq
;=============================================================================
_init_pi_reg_iq:
LDP #_i_q
LACC #0
SACL _iq_1
SACL _iq_ref_1
SACL _i_q
SACL _i_q_ref
SACL _e_iq_1
SACL _I_iq_1_high
SACL _I_iq_1_low
SACL _u_q_ref
SACL _uq_ref_low
SACL _e_iq_1
SACL _e_iq_1
RET
;==============================================================================
; Routine Name: _pi_reg_iq
; ------------------------
; Description: Implements iq current PI controller
;
; OVM=1
; dyi=sat(yi-yim1)
; dyri=sat(yri-yrim1)
; ei=sat(eim1+dyri-dyi)
; dyri=ei-eim1+dyi
; Pi=satsfl(Kps*dyri-Kps*dyi)
; uim1=ui=sat(Pi+Iim1+uim1)
; Iim1=satsfl(Kis*ei)
; yrim1=yrim1+dyri
; yim1=yi
; eim1=ei
; OVM=0
; -------------------------
; Calling Convention: extern void _pi_reg_iq();
;==============================================================================
; +--------------------------------------------------------------------------
; | Input global variables: |
; +--------------------------------------------------------------------------+
; | _i_q_ref | reference value of i_q current at step i (yri) |
; | _i_q | feedback value of i_q current at step i (yi) |
; | _Kps_iq | scaled Kp coeff: Kps = Kp / (2^scalKp) so that |
; | | -1 < Kps < +1 |
; | _sf_P_iq | sf_P_iq = scalKp |
; | _Kis_iq | scaled Ki coeff: Kis = Ki / (2^scalKi) so that |
; | | -1 < Kis < +1 |
; | _sf_I_iq | sf_I_iq = scalKi |
; | _iq_ref_1 | reference value of i_q current at step i-1 (yrim1) |
; | _iq_1 | feedback value of i_q current at step i-1 (yim1) |
; | _e_iq_1 | error at step i-1 |
; | _I_iq_1_high | integral term at step i-1, (16 MSB) |
; | _I_iq_1_low | integral term at step i-1, (16 LSB) |
; +--------------------------------------------------------------------------+
; | Input local variables: |
; +--------------------------------------------------------------------------+
; | dif_iq_ref | difference between actual and old reference |
; | dif_iq | difference between actual and old feedback |
; | e_iq | error at step i |
; +--------------------------------------------------------------------------+
; | Output global variables: |
; +--------------------------------------------------------------------------+
; | _u_q_ref | output of the regulator (16 MSB) |
; | _uq_ref_low | output of the regulator (16 LSB) |
; +--------------------------------------------------------------------------+
;------------------------------------------------------------------------------
.global _pi_reg_iq
;==============================================================================
_pi_reg_iq:
SETC SXM
LDP #_i_q
SETC OVM ; set overflow protection mode
LACC _i_q,16
SUB _iq_1,16
SACH dif_iq ; dyi=sat(yi-yim1)
LACC _i_q_ref,16
SUB _iq_ref_1,16
SACH dif_iq_ref ; dyri=sat(yri-yrim1)
SUB dif_iq,16 ; ACC=dyri-dyi
ADD _e_iq_1,16
SACH e_iq ; ei=sat(eim1+dyri-dyi)
SUB _e_iq_1,16
ADD dif_iq,16
SACH dif_iq_ref ; dyri=ei-eim1+dyi
LT _Kps_iq
MPY dif_iq_ref ; Kps*dyri
PAC
MPY dif_iq ; Kps*dyi
SPAC ; Kps*dyri-Kps*dyi
SATSFL _sf_P_iq ; ACC=Pi=satsfl(Kps*dyri-Kps*dyi) in Q31 format
CLRC SXM
ADD _I_iq_1_low
ADD _uq_ref_low
SETC SXM
ADD _I_iq_1_high,16
ADD _u_q_ref,16
sat_reg_out C_SAT_U_Q_REF ; macro that saturates regulator's output
SACH _u_q_ref ; uim1=ui=sat(Pi+Iim1+uim1)
SACL _uq_ref_low
LT _Kis_iq
MPY e_iq
PAC ; Kis*ei
SATSFL _sf_I_iq ; ACC=Iim1=satsfl(Kis*ei)
SACH _I_iq_1_high ; Iim1=satsfl(Kis*ei)
SACL _I_iq_1_low
LACC _iq_ref_1,16
ADD dif_iq_ref,16
SACH _iq_ref_1 ; yrim1=yrim1+dyri
LACC _i_q
SACL _iq_1 ; yim1=yi
LACC e_iq
SACL _e_iq_1 ; eim1=ei
CLRC OVM ; disable overflow protection mode
RET
;=============================================================================
; Routine Name: _init_pi_reg_pos
; ------------------------------
; Description: Setup of position, PI controller variables
; ------------------------------
; Calling Convention: extern void _init_pi_reg_pos();
;=============================================================================
.global _init_pi_reg_pos
;=============================================================================
_init_pi_reg_pos:
LDP #_position
LACC #0
SACL _pos_ref_1
SACL _pos_1
SACL _pos_ref
SACL _pos
SACL _e_pos_1
SACL _I_pos_1_high
SACL _I_pos_1_low
SACL _omg_ref
SACL _omg_ref_low
SACL _e_pos_1
SACL _e_pos_1
RET
;=============================================================================
; Routine Name: _pi_reg_pos
; -------------------------
; Description: Implements position PI controller
;
; OVM=1
; dyi=sat(yi-yim1)
; dyri=sat(yri-yrim1)
; ei=sat(eim1+dyri-dyi)
; dyri=ei-eim1+dyi
; Pi=satsfl(Kps*dyri-Kps*dyi)
; uim1=ui=sat(Pi+Iim1+uim1)
; Iim1=satsfl(Kis*ei)
; yrim1=yrim1+dyri
; yim1=yi
; eim1=ei
; OVM=0
; --------------------------
; Calling Convention: extern void _pi_reg_pos();
;==============================================================================
; +--------------------------------------------------------------------------+
; | Input global variables: |
; +--------------------------------------------------------------------------+
; | _pos_ref | reference value of the position at step i (yri) |
; | _pos | feedback value of the position at step i (yi) |
; | _Kps_pos | scaled Kp coeff: Kps = Kp / (2^scalKp) so that |
; | | -1 < Kps < +1 |
; | _Kis_pos | scaled Ki coeff: Kis = Ki / (2^scalKi) so that |
; | | -1 < Kis < +1 |
; | _pos_ref_1 | reference value of the position at step i-1 (yrim1) |
; | _pos_1 | feedback value of the position at step i-1 (yim1) |
; | _e_pos_1 | error at step i-1 |
; | _I_pos_1_high | integral term at step i-1, (16 MSB) |
; | _I_pos_1_low | integral term at step i-1, (16 LSB) |
; +--------------------------------------------------------------------------+
; | Input local variables: |
; +--------------------------------------------------------------------------+
; | dif_pos_ref | difference between actual and old reference |
; | dif_pos | difference between actual and old feedback |
; | e_pos | error at step i |
; +--------------------------------------------------------------------------+
; | Output global variables: |
; +--------------------------------------------------------------------------+
; | _omg_ref | output of the regulator (16 MSB) |
; | _omg_ref_low | output of the regulator (16 LSB) |
; +--------------------------------------------------------------------------+
;------------------------------------------------------------------------------
.global _pi_reg_pos
;==============================================================================
_pi_reg_pos:
SETC SXM
LDP #_position
SETC OVM ; set overflow protection mode
LACC _pos_ref,16
SUB _pos_ref_1,16
SACH dif_pos_ref ; dyri=sat(yri-yrim1)
LACC _pos,16
SUB _pos_1,16
SACH dif_pos ; dyi=sat(yi-yim1)
SUB dif_pos_ref,16 ; ACC=dyri-dyi
NEG
ADD _e_pos_1,16
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -