📄 imvc07_a.asm
字号:
SACH e_pos ; ei=sat(eim1+dyri-dyi)
SUB _e_pos_1,16
ADD dif_pos,16
SACH dif_pos_ref ; dyri=ei-eim1+dyi
LT _Kps_pos
MPY dif_pos_ref ; Kps*dyri
PAC
MPY dif_pos ; Kps*dyi
SPAC ; Kps*dyri-Kps*dyi
CLRC SXM
ADD _I_pos_1_low
ADD _omg_ref_low
SETC SXM
ADD _I_pos_1_high,16
ADD _omg_ref,16
sat_reg_out C_SAT_OMG_REF ; macro that saturates regulator's output
SACH _omg_ref ; uim1=ui=sat(Pi+Iim1+uim1)
SACL _omg_ref_low
LT _Kis_pos
MPY e_pos
PAC ; Kis*ei
SACH _I_pos_1_high ; Iim1=satsfl(Kis*ei)
SACL _I_pos_1_low
LACC _pos_ref_1,16
ADD dif_pos_ref,16
SACH _pos_ref_1 ; yrim1=yrim1+dyri
LACC _pos
SACL _pos_1 ; yim1=yi
LACC e_pos
SACL _e_pos_1 ; eim1=ei
CLRC OVM ; disable overflow protection mode
RET
;=============================================================================
; Routine Name: _init_pi_reg_omg
; ------------------------------
; Description: Setup of speed, PI controller variables
; ------------------------------
; Calling Convention: extern void _init_pi_reg_omg();
;=============================================================================
.global _init_pi_reg_omg
;=============================================================================
_init_pi_reg_omg:
LDP #_omg
LACC #0
SACL _omg_ref_1
SACL _omg_1
SACL _omg_ref
SACL _omg
SACL _e_omg_1
SACL _I_omg_1_high
SACL _I_omg_1_low
SACL _i_q_ref
SACL _iq_ref_low
RET
;=============================================================================
; Routine Name: _pi_reg_omg
; -------------------------
; Description: Implements speed 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_omg();
;==============================================================================
; +--------------------------------------------------------------------------+
; | Input global variables: |
; +--------------------------------------------------------------------------+
; | _omg_ref | reference value of the speed at step i (yri) |
; | _omg | feedback value of the speed at step i (yi) |
; | _Kps_omg | scaled Kp coeff: Kps = Kp / (2^scalKp) so that |
; | | -1 < Kps < +1 |
; | _sf_P_omg | sf_P_omg = scalKp |
; | _Kis_omg | scaled Ki coeff: Kis = Ki / (2^scalKi) so that |
; | | -1 < Kis < +1 |
; | _sf_I_omg | sf_I_omg = scalKi |
; | _omg_ref_1 | reference value of the speed at step i-1 (yrim1) |
; | _omg_1 | feedback value of the speed at step i-1 (yim1) |
; | _e_omg_1 | error at step i-1 |
; | _I_omg_1_high | integral term at step i-1, (16 MSB) |
; | _I_omg_1_low | integral term at step i-1, (16 LSB) |
; +--------------------------------------------------------------------------+
; | Input local variables: |
; +--------------------------------------------------------------------------+
; | dif_omg_ref | difference between actual and old reference |
; | dif_omg | difference between actual and old feedback |
; | e_omg | error at step i |
; +--------------------------------------------------------------------------+
; | Output global variables: |
; +--------------------------------------------------------------------------+
; | _i_q_ref | output of the regulator (16 MSB) |
; | _iq_ref_low | output of the regulator (16 LSB) |
; +--------------------------------------------------------------------------+
;------------------------------------------------------------------------------
.global _pi_reg_omg
;==============================================================================
_pi_reg_omg:
SETC SXM
LDP #_omg
SETC OVM ; set overflow protection mode
LACC _omg,16
SUB _omg_1,16
SACH dif_omg ; dyi=sat(yi-yim1)
LACC _omg_ref,16
SUB _omg_ref_1,16
SACH dif_omg_ref ; dyri=sat(yri-yrim1)
SUB dif_omg,16 ; ACC=dyri-dyi
ADD _e_omg_1,16
SACH e_omg ; ei=sat(eim1+dyri-dyi)
SUB _e_omg_1,16
ADD dif_omg,16
SACH dif_omg_ref ; dyri=ei-eim1+dyi
LT _Kps_omg
MPY dif_omg_ref ; Kps*dyri
PAC
MPY dif_omg ; Kps*dyi
SPAC ; Kps*dyri-Kps*dyi
SATSFL _sf_P_omg ; ACC=Pi=satsfl(Kps*dyri-Kps*dyi) in Q31 format
CLRC SXM
ADD _I_omg_1_low
ADD _iq_ref_low
SETC SXM
ADD _I_omg_1_high,16
ADD _i_q_ref,16
sat_reg_out C_SAT_I_Q_REF ; macro that saturates regulator's output
SACH _i_q_ref ; uim1=ui=sat(Pi+Iim1+uim1)
SACL _iq_ref_low
LT _Kis_omg
MPY e_omg
PAC ; Kis*ei
SATSFL _sf_I_omg ; ACC=Iim1=satsfl(Kis*ei)
SACH _I_omg_1_high ; Iim1=satsfl(Kis*ei)
SACL _I_omg_1_low
LACC _omg_ref_1,16
ADD dif_omg_ref,16
SACH _omg_ref_1 ; yrim1=yrim1+dyri
LACC _omg
SACL _omg_1 ; yim1=yi
LACC e_omg
SACL _e_omg_1 ; eim1=ei
CLRC OVM ; disable overflow protection mode
RET
;=============================================================================
; Routine Name: _start_encoder
; ----------------------------
; Description: Configure CAPCON to enable QEP circuit
; ----------------------------
; Calling Convention: extern void start_encoder();
;=============================================================================
; +-----------------+-------------------+-----------+
; | Variables | on Entry | on Exit |
; +-----------------+-------------------+-----------+
; | ACC | xx |CAPCON Reg.|
; | DP | xx | DP_EV |
; +-----------------+-------------------+-----------+
;-----------------------------------------------------------------------------
CAPCON_QEP_EN .set 0E000h ; enable QEP in CAPCON register
.global _start_encoder
;=============================================================================
_start_encoder:
LDP #DP_EV
SETBIT T2CON,SETB6 ;start GPT2 to count QEP pulses
LACC CAPCONA
OR #CAPCON_QEP_EN
SACL CAPCONA ; enable QEP in CAPCON register
RET
;=============================================================================
; Routine Name: _read_encoder
; ---------------------------
; Description: Reads GPT2 counter (QEP capture pulses) and computes position.
; The encoder position is loaded in the variable position
; ---------------------------
; Calling Convention: extern void _read_encoder();
;=============================================================================
; +-----------------+-------------------+-----------+
; | Variables | on Entry | on Exit |
; +-----------------+-------------------+-----------+
; | ACC | xx | _position |
; | DP | xx | DP_EV |
; +-----------------+-------------------+-----------+
;-----------------------------------------------------------------------------
.global _read_encoder
;=============================================================================
_read_encoder:
LDP #DP_EV
LACC T2CNT ; read GPT2 current counter value
LDP #_position
SACL _position ;store position
RET
;=============================================================================
; Routine Name: _tabcdq
; ---------------------
; Description: Transforms coordinates from abc to dq frame
; input of the transformation routine are current from phase A and B
; and position of the rotor flux(theta)and output are transformed currents
; from the synchronous reference frame.
; ---------------------
; Calling Convention: extern void tabcdq();
;=============================================================================
Cas1 .word 9549 ; Constant Ca=1/sqrt(3) scaled by *2^-1
Cbs1 .word 18919 ; Constant Cb=2/sqrt(3) scaled by *2^-1
Cnalpha .word -16284 ; Constant Calpha=-0.5, negative
Cbeta .word 28373 ; Constant Cbeta=sqrt(3)/2
;=============================================================================
.global _tabcdq
;=============================================================================
_tabcdq:
LDP #_theta
; call sine to obtain sin_theta
LACC _theta ; argument
SACL *+ ; push argument on the stack
CALL _sine ; call function
MAR *- ; restore SP
LDP #_theta
SACL _sin_theta ; store sin_theta
; cosine(theta) = sine(theta + PI/2)
LACC _theta
ADD #C_PI_2 ; obtain augument
SACL *+ ; push augument on the stack
CALL _sine ; call function
MAR *- ; restore SP
LDP #_theta
SACL _cos_theta ; store cos_theta
; transform abc2alphabeta
SETC OVM ; OPERATIONS WIIL BE MADE UNDER OVM = 1
; _i_alpha = _i_a
LACC _i_a
SACL _i_alpha
; _i_beta = (Cas1*_i_a + Cbs1*_i_b)*2
MAC Cbs1,_i_b
LACL #0
MAC Cas1,_i_a
APAC
sat2
SACH _i_beta, 2
; transform alphabeta2dq
; _i_d = _i_alpha*_cos_theta + _i_beta*_sin_theta
MPY _cos_theta ; P still loaded with _i_a(lpha)
LTP _i_beta
MPY _sin_theta
APAC
sat1
SACH _i_d, 1
; i_q = -_i_alpha*_sin_theta + _i_beta*_cos_theta
MPY _cos_theta ; P still loaded with _i_beta
LTP _i_alpha
MPY _sin_theta
SPAC
sat1
SACH _i_q, 1
CLRC OVM ; OPERATIONS WERE MADE UNDER OVM = 1
RET
;=============================================================================
; Routine Name: _tdqabc
; ---------------------
; Description: Transforms coordinates from dq to abc frame
; Input of the routine are dq reference voltages (output of the dq axis
; current controllers and the rotation angle of the rotor flux(theta)
; and output are the reference voltages in the natural frame of the motor
; ---------------------
; Calling Convention: extern void tdqabc();
;=============================================================================
.global _tdqabc
;=============================================================================
_tdqabc:
LDP #_theta
; transform dq2alphabeta
SETC OVM ; OPERATIONS WIIL BE MADE UNDER OVM = 1
; _u_beta = _u_d*_sin_theta + _u_q*_cos_theta;
LT _u_q_ref
MPY _cos_theta
LTP _u_d_ref
MPY _sin_theta
APAC
sat1
SACH _u_beta_ref, 1
; _u_alpha = _u_d*_cos_theta - _u_q*_sin_theta;
MPY _cos_theta ; P still loaded with _u_d
LTP _u_q_ref
MPY _sin_theta
SPAC
sat1
SACH _u_alpha_ref, 1
; transform alphabeta2abc
; _u_a = _u_alpha;
SACH _u_a_ref, 1
; _u_b = Cnalpha*_u_alpha + Cbeta*_u_beta;
MAC Cnalpha,_u_alpha_ref
LACL #0
MAC Cbeta,_u_beta_ref
APAC
sat1
SACH _u_b_ref, 1
; _u_c = Cnalpha*_u_alpha - Cbeta*_u_beta;
MAC Cnalpha,_u_alpha_ref
LACL #0
MAC Cbeta,_u_beta_ref
SPAC
sat1
SACH _u_c_ref, 1
CLRC OVM ; OPERATIONS WERE MADE UNDER OVM = 1
RET
;=============================================================================
; Routine Name: _update_pwm
;-------------------------
; Description: update the compare registers with ON time value for the power stage
;-------------------------
; Calling Convention: extern void update_pwm ();
;=============================================================================
.global _update_pwm
;=============================================================================
_update_pwm:
SETC SXM
LDP #_pwm_period
LT _pwm_period
MPY _u_a_ref
PAC
ADD _pwm_period, 15 ; += period/2
LDP #DP_EV ; Event Manager Data Page Pointer
SACH CMPR1 ; CMPR1 = (period * (1 + u_a_ref)) /2
LDP #_pwm_period
MPY _u_b_ref
PAC
ADD _pwm_period, 15 ; += period/2
LDP #DP_EV ; Event Manager Data Page Pointer
SACH CMPR2 ; CMPR2 = (period * (1 + u_b_ref)) /2
LDP #_pwm_period
MPY _u_c_ref
PAC
ADD _pwm_period, 15 ; += period/2
LDP #DP_EV ; Event Manager Data Page Pointer
SACH CMPR3 ; CMPR3 = (period * (1 + u_c_ref)) /2
RET
;=============================================================================
; Routine Name: _start_pwm
; ------------------------
; Description: Enables PWM generation
; ------------------------
; Calling Convention: extern void start_pwm();
;=============================================================================
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -