📄 pmsm3_1.asm
字号:
LDP #Mfunc_c1
BLDD #Ta,Mfunc_c1
BLDD #Tb,Mfunc_c2
BLDD #Tc,Mfunc_c3
CALL FC_PWM_DRV
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase2_inc_build)
;---------------------------------------------------------
LDP #target_value
BLDD #speed_ref,target_value
CALL RAMP_CNTL
LDP #rmp_freq
BLDD #setpt_value,rmp_freq
CALL RAMP_GEN
LDP #theta_ip
BLDD #rmp_out,theta_ip
BLDD #Vd_testing,ipark_D
BLDD #Vq_testing,ipark_Q
CALL I_PARK
LDP #Ualfa
BLDD #ipark_d,Ualfa
BLDD #ipark_q,Ubeta
CALL SVGEN_DQ
LDP #Mfunc_c1
BLDD #Ta,Mfunc_c1
BLDD #Tb,Mfunc_c2
BLDD #Tc,Mfunc_c3
CALL FC_PWM_DRV
CALL ILEG2_DCBUS_DRV
LDP #clark_a
BLDD #Imeas_a,clark_a
BLDD #Imeas_b,clark_b
CALL CLARKE
LDP #park_d
BLDD #clark_d,park_d
BLDD #clark_q,park_q
BLDD #rmp_out,theta_p
CALL PARK
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase3_inc_build)
;---------------------------------------------------------
LDP #target_value
BLDD #speed_ref,target_value
CALL RAMP_CNTL
LDP #rmp_freq
BLDD #setpt_value,rmp_freq ; using step ramp control
CALL RAMP_GEN
LDP #id_ref
BLDD #Id_ref,id_ref
BLDD #park_D,id_fdb
CALL PID_REG3_ID
LDP #iq_ref
BLDD #Iq_ref,iq_ref
BLDD #park_Q,iq_fdb
CALL PID_REG3_IQ
LDP #theta_ip
BLDD #ud_out,ipark_D
BLDD #uq_out,ipark_Q
BLDD #rmp_out,theta_ip
CALL I_PARK
LDP #Ualfa
BLDD #ipark_d,Ualfa
BLDD #ipark_q,Ubeta
CALL SVGEN_DQ
LDP #Mfunc_c1
BLDD #Ta,Mfunc_c1
BLDD #Tb,Mfunc_c2
BLDD #Tc,Mfunc_c3
CALL FC_PWM_DRV
CALL ILEG2_DCBUS_DRV
LDP #clark_a
BLDD #Imeas_a,clark_a
BLDD #Imeas_b,clark_b
CALL CLARKE
LDP #park_d
BLDD #clark_d,park_d
BLDD #clark_q,park_q
BLDD #rmp_out,theta_p
CALL PARK
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase4_inc_build)
;---------------------------------------------------------
LDP #target_value
BLDD #speed_ref,target_value
CALL RAMP_CNTL
LDP #rmp_freq
BLDD #setpt_value,rmp_freq
CALL RAMP_GEN
LDP #id_ref
BLDD #Id_ref,id_ref
BLDD #park_D,id_fdb
CALL PID_REG3_ID
LDP #iq_ref
BLDD #Iq_ref,iq_ref
BLDD #park_Q,iq_fdb
CALL PID_REG3_IQ
; checking the lockrtr_flg here
POINT_B0
LACC lockrtr_flg ; lockrtr_flg=0 for constant current mode (angle=0)
BCND NON_CC_MODE,EQ ; Branch to NON_CC_MODE if lockrtr_flg=0
CC_MODE
LDP #theta_ip
BLDD #ud_out,ipark_D
BLDD #uq_out,ipark_Q
SPLK #0000h,theta_ip
CALL I_PARK
LDP #Ualfa
BLDD #ipark_d,Ualfa
BLDD #ipark_q,Ubeta
CALL SVGEN_DQ
LDP #Mfunc_c1
BLDD #Ta,Mfunc_c1
BLDD #Tb,Mfunc_c2
BLDD #Tc,Mfunc_c3
CALL FC_PWM_DRV
CALL ILEG2_DCBUS_DRV
LDP #clark_a
BLDD #Imeas_a,clark_a
BLDD #Imeas_b,clark_b
CALL CLARKE
LDP #park_d
BLDD #clark_d,park_d
BLDD #clark_q,park_q
SPLK #0000h,theta_p
CALL PARK
B SKIP_CC_MODE
NON_CC_MODE
LDP #theta_ip
BLDD #ud_out,ipark_D
BLDD #uq_out,ipark_Q
BLDD #rmp_out,theta_ip
CALL I_PARK
LDP #Ualfa
BLDD #ipark_d,Ualfa
BLDD #ipark_q,Ubeta
CALL SVGEN_DQ
LDP #Mfunc_c1
BLDD #Ta,Mfunc_c1
BLDD #Tb,Mfunc_c2
BLDD #Tc,Mfunc_c3
CALL FC_PWM_DRV
CALL ILEG2_DCBUS_DRV
LDP #clark_a
BLDD #Imeas_a,clark_a
BLDD #Imeas_b,clark_b
CALL CLARKE
LDP #park_d
BLDD #clark_d,park_d
BLDD #clark_q,park_q
BLDD #rmp_out,theta_p
CALL PARK
SKIP_CC_MODE
CALL QEP_THETA_DRV
LDP #theta_elec_fr
BLDD #theta_elec,theta_elec_fr
CALL SPEED_FRQ
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (phase5_inc_build)
;---------------------------------------------------------
LDP #target_value
BLDD #speed_ref,target_value
CALL RAMP_CNTL
POINT_B0
LACC sp_loop_max
SUB sp_loop_cnt
BCND SKIP_SP_PID,GEQ
SPLK #0,sp_loop_cnt ; reset sp_loop_cnt
LDP #spd_ref
; BLDD #setpt_value,spd_ref
BLDD #speed_ref,spd_ref
BLDD #speed_frq,spd_fdb
CALL PID_REG3_SPD
SKIP_SP_PID
LACC sp_loop_cnt
ADD #1h
SACL sp_loop_cnt ; keep counting sp_loop_cnt
LDP #id_ref
BLDD #Id_ref,id_ref
BLDD #park_D,id_fdb
CALL PID_REG3_ID
LDP #iq_ref
BLDD #t_out,iq_ref
BLDD #park_Q,iq_fdb
CALL PID_REG3_IQ
LDP #theta_ip
BLDD #ud_out,ipark_D
BLDD #uq_out,ipark_Q
BLDD #theta_elec,theta_ip
CALL I_PARK
LDP #Ualfa
BLDD #ipark_d,Ualfa
BLDD #ipark_q,Ubeta
CALL SVGEN_DQ
LDP #Mfunc_c1
BLDD #Ta,Mfunc_c1
BLDD #Tb,Mfunc_c2
BLDD #Tc,Mfunc_c3
CALL FC_PWM_DRV
CALL ILEG2_DCBUS_DRV
LDP #clark_a
BLDD #Imeas_a,clark_a
BLDD #Imeas_b,clark_b
CALL CLARKE
LDP #park_d
BLDD #clark_d,park_d
BLDD #clark_q,park_q
BLDD #theta_elec,theta_p
CALL PARK
CALL QEP_THETA_DRV
LDP #theta_elec_fr
BLDD #theta_elec,theta_elec_fr
CALL SPEED_FRQ
.endif
;---------------------------------------------------------
;---------------------------------------------------------
;Checking bandwidth of ISR
SETC XF
CALL DAC_VIEW_DRV
; Change reference speed from pu value to rpm value (Q15 -> Q0)
POINT_B0
SPLK #6000, GPR1 ; GPR1 = base rpm speed (Q0)
LT GPR1 ; TREG = GPR1 (Q0)
MPY speed_ref ; PREG = GPR1*speed_ref (Q15)
PAC ; ACC = GPR1*speed_ref (Q15)
SACH speed_ref_rpm,1 ; speed_ref_rpm = GPR1*speed_ref (Q0)
; Check enable_flg to enable/disable PWM drive signals
LACC enable_flg
BCND ENABLE_DRIVE,GT
CALL DIS_DRIVE ; Disable drive
B SKIP_DRIVE
ENABLE_DRIVE
CALL EN_DRIVE ; Enable drive
SKIP_DRIVE
;Checking bandwidth of ISR
SETC XF
;=========================================================
;End main section of ISR
;=========================================================
;Context restore regs
END_ISR:
POINT_PG0
MAR *, AR1 ; make stack pointer active
MAR *- ; point to top of stack
;More context restore if needed
PSHD *- ; restore TOS
LAR AR6,*- ; restore AR6
LACL *- ; Restore Acc low
ADD *-,16 ; Restore Acc high
LST #0, *- ; Restore ST0
LST #1, *- ; Restore ST1 and pointer
EINT
RET
.if x243|x2407
*******************************************************
* INT1 - PDP & Sys Peripheral Ints
* Enabled: PDPINT
*******************************************************
PDPINT_ID .set 020h ; PDPINT vector ID
PDPINT_CLR .set 01h ; PDPINT flag clear
_c_int1:
MAR *,AR1 ; Save context
MAR *+ ; point to a guaranteed unused location
SST #1, *+ ; save ST1
SST #0, *+ ; save ST0
SACH *+ ; save acc high
SACL *+ ; save acc low and point at an unused loca.
;More context save if needed
SETC SXM ; set sign extension mode
CLRC OVM ; clear overflow mode
LDP #PIVR>>7 ; set DP
LACC PIVR ; load peripheral int vector/ID/offset
SUB #PDPINT_ID ; PDPINT?
BCND PDP_INT,EQ ; to PDPISR if zero
CALL PHANTOM ; got a phantom int if not
B REST_INT1 ; return
PDP_INT
LDP #EVIFRA>>7 ; set DP
SPLK #PDPINT_CLR,EVIFRA ; Clear PDPINT flag
LDP #pdpint_flg ; set DP
SPLK #1,pdpint_flg ; set flag
REST_INT1
;More context restore if needed
MAR *, AR1 ; make stack pointer active
MAR *- ; point to top of stack
LACL *- ; Restore Acc low
ADD *-,16 ; Restore Acc high
LST #0, *- ; restore ST0
LST #1, *- ; restore ST1 and pointer
EINT
RET ; return
*** END INT1 - PDP & Sys Peripheral Ints
.endif
*******************************************************
* INT4 - EV group C Ints
* Enabled: Capture 3 int as QEP index
*******************************************************
CAP3_ID .set 035H ; Capture 3 int vector ID
CAP3_CLR .set 04H ; Capture 3 int flag clear
QEP_ISR:
MAR *,AR1 ; Save context
MAR *+ ; point to a guaranteed unused location
SST #1, *+ ; save ST1
SST #0, *+ ; save ST0
SACH *+ ; save acc high
SACL *+ ; save acc low and point to an unused loca.
;More context save if needed
.if x243 | x2407
LDP #PIVR>>7 ; set DP
LACC PIVR ; load peripheral int vector/ID/offset
.endif
.if x240
LDP #EVIVRC>>7
lACC EVIVRC
.endif
SUB #CAP3_ID ; Capture 1 int?
BCND CAP3_INT,EQ ; to PDPISR if zero
CALL PHANTOM ; got a phantom int if not
B END_INT4 ; return
CAP3_INT
.if x243 | x2407
LDP #EVIFRC>>7
SPLK #CAP3_CLR,EVIFRC ; Clear Capture 1 int flag
.endif
CALL QEP_INDEX_ISR_DRV ; QEP index routine
;=========================================================
;End main section of ISR INT4 -EV GROUP C INTS
;=========================================================
;Context restore regs
END_INT4
POINT_PG0
MAR *, AR1 ; make stack pointer active
MAR *- ; point to top of stack
;More context restore if needed
LACL *- ; Restore Acc low
ADD *-,16 ; Restore Acc high
LST #0, *- ; restore ST0
LST #1, *- ; restore ST1 and pointer
EINT
RET ; return
;==============================================================================
; I S R - PHANTOM
;
; Description: Dummy ISR, used to trap spurious interrupts.
;
; Modifies:
;
; Last Update: 16-06-95
;==============================================================================
PHANTOM B PHANTOM
PHANTOM1 B PHANTOM1
PHANTOM2 B PHANTOM2
PHANTOM3 B PHANTOM3
PHANTOM4 B PHANTOM4
PHANTOM5 B PHANTOM5
PHANTOM6 B PHANTOM6⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -