📄 zhtest3_2.asm
字号:
;==============================================================================
; 文件名: Zhtest3_2.ASM
;
; 系统名: Zhtest3_2（永磁同步电机FOC控制系统）
;
; 功能: 带速度传感器的三相永磁同步电机磁场定向控制
;
; 公司: 达盛科技
;
; 目标CPU: x2407
;
; 修改记录:
; ____________________________________________________________________________
; 最后更新时间： 2005.08.21 版本：Ver1.0
;==============================================================================
;******************************************************************************
; 系统选项
;******************************************************************************
real_time .set 1 ; 选择'1'代表实时模式, '0'代表非实时模式
;******************************************************************************
;-----------------------------------------------------------------------------
step1 .set 1 ; 矢量计算和pwm产生模块测试
step2 .set 0 ; 电流测量模块测试
step3 .set 0 ; 两组 PI电流调节器测试
step4 .set 0 ; QEP模块和角度计算模块测试
step5 .set 0 ; 速度闭环完整系统测试
;-----------------------------------------------------------------------------
; 全局变量声明
;-----------------------------------------------------------------------------
.include "x24x_app.h"
.global MON_RT_CNFG
.ref SYS_INIT
.ref RAMP_CNTL, RAMP_CNTL_INIT ; 子程序调用
.ref target_value ; 输入
.ref rmp_dly_max, rmp_lo_limit ; 输入参数
.ref rmp_hi_limit ; 输入参数
.ref setpt_value, s_eq_t_flg ; 输出
.ref FC_PWM_DRV, FC_PWM_DRV_INIT ;子程序调用
.ref Mfunc_c1, Mfunc_c2, Mfunc_c3, Mfunc_p ;输入
.ref CLARKE, CLARKE_INIT ;子程序调用
.ref clark_a, clark_b ;输入
.ref clark_d, clark_q ;输出
.ref ILEG2_DCBUS_DRV, ILEG2_DCBUS_DRV_INIT ;子程序调用
.ref Ch_sel, Imeas_a_gain, Imeas_b_gain, Vdc_meas_gain ;输入参数
.ref Imeas_a_offset, Imeas_b_offset, Vdc_meas_offset ;输入参数
.ref Imeas_a,Imeas_b,Imeas_c ;输出
.ref Vdc_meas ;输出
.ref I_PARK, I_PARK_INIT ;子程序调用
.ref ipark_D, ipark_Q, theta_ip ;输入
.ref ipark_d, ipark_q ;输出
.ref PARK, PARK_INIT ;子程序调用
.ref park_d, park_q, theta_p ;输入
.ref park_D, park_Q ;输出
.ref RAMP_GEN, RAMP_GEN_INIT ;子程序调用
.ref rmp_gain, rmp_offset, rmp_freq ;输入
.ref step_angle_max ;输入
.ref rmp_out ;输出
.ref SVGEN_DQ,SVGEN_DQ_INIT ;子程序调用
.ref Ualfa,Ubeta ;输入
.ref Ta,Tb,Tc ;输出
.ref PID_REG3_ID,PID_REG3_ID_INIT ; 子程序调用
.ref id_fdb,id_ref ; 输入
.ref ud_out ; 输出
.ref PID_REG3_IQ,PID_REG3_IQ_INIT ; 子程序调用
.ref iq_fdb,iq_ref ; 输入
.ref uq_out ; 输出
.ref PID_REG3_SPD,PID_REG3_SPD_INIT ; 子程序调用
.ref spd_fdb,spd_ref ; 输入
.ref t_out ; 输出
.ref EN_DRIVE,DIS_DRIVE,DRIVE_INIT ; 子程序调用
.ref QEP_THETA_DRV,QEP_THETA_DRV_INIT ; 子程序调用
.ref polepairs,cal_angle,mech_scale ; 输入
.ref theta_elec,theta_mech,dir_QEP ; 输出
.ref QEP_INDEX_ISR_DRV ; 子程序调用
.ref index_sync_flg,QEP_cnt_idx ; 输出
.ref SPEED_FRQ, SPEED_FRQ_INIT ; 子程序调用
.ref theta_elec_fr ; 输入
.ref speed_frq, speed_frq_rpm ; 输出
.ref DATA_LOG, DATA_LOG_INIT ; 子程序调用
;
;-----------------------------------------------------------------------------
; 局部变量定义
;-----------------------------------------------------------------------------
.def GPR0 ;通用寄存器.
.def GPR1
.def GPR2
.def GPR3
.def GPR4
.def COMCON
.bss GPR0,1 ;通用寄存器.
.bss GPR1,1
.bss GPR2,1
.bss GPR3,1
.bss GPR4,1
.bss speed_ref,1 ;输入参数及标志位
.bss speed_ref_rpm,1
.bss Vd_testing,1
.bss Vq_testing,1
.bss Id_ref,1
.bss Iq_ref,1
.bss lockrtr_flg,1
.bss enable_flg,1
.bss cl_flg,1
.bss pdpint_flg,1
.bss sp_loop_cnt,1 ;计数器参数
.bss sp_loop_max,1
.bss isr_ticker,1
.bss data_tmp,1 ;临时变量
;==============================================================================
; 中断向量表
;==============================================================================
.include "c200mnrt.i" ; 包含汇编条件选项.
.sect "vectors"
.def _c_int0
.def QEP_ISR ; int4-事件管理器 C 组中断服务
RESET B _c_int0 ; 00
INT1 B _c_int1 ; 02
INT2 B T1UF_ISR ; 04
INT3 B PHANTOM ; 06
INT4 B QEP_ISR ; 08
INT5 B PHANTOM ; 0A
INT6 B PHANTOM ; 0C
.include "rtvecs.h"
; 注意 : 上面的 include 行 必须放置在用户可设定的变量之后
;==============================================================================
; 主程序代码开始
;==============================================================================
.text
_c_int0: ;公共初始化子程序调用
CALL SYS_INIT ;系统初始化
CALL DATA_LOG_INIT ;显示数据获取程序初始化
CALL RAMP_GEN_INIT ;开环磁链产生模块初始化
CALL RAMP_CNTL_INIT ;磁链控制模块初始化
CALL SVGEN_DQ_INIT ;电压空间矢量模块初始化
CALL FC_PWM_DRV_INIT ;PWM波形发生模块初始化
CALL ILEG2_DCBUS_DRV_INIT ;电流采样模块初始化
CALL CLARKE_INIT ;CLARKE变换模块初始化
CALL I_PARK_INIT ;PARK逆变换模块初始化
CALL PARK_INIT ;PARK变换模块初始化
CALL PID_REG3_ID_INIT ;d轴电流调节模块初始化
CALL PID_REG3_IQ_INIT ;q轴电流调节模块初始化
CALL PID_REG3_SPD_INIT ;速度调节模块初始化
CALL DRIVE_INIT ;驱动使能模块初始化
CALL QEP_THETA_DRV_INIT ;QEP及角度计算模块初始化
CALL SPEED_FRQ_INIT ;速度计算模块初始化
;------------------------------------------------------------------------------------------
;系统时基初始化
;------------------------------------------------------------------------------------------
;时基通过 T1 下溢中断产生，初始化在pwm初始化程序FC_PWM_DRV_INIT中完成
;----------------------------------------------------------
; 实时监控程序初始化
;----------------------------------------------------------
;---实时模式选项---------------
.if (real_time)
CALL MON_RT_CNFG ;实时监控程序
.endif
;----------------------------------
.if (real_time) ; 等待手动使能
POINT_B0
SPLK #1,enable_flg
SPLK #0,cl_flg
WAIT_ENABLE LACC enable_flg
BCND WAIT_ENABLE,EQ
.endif
CALL EN_DRIVE ; 驱动使能程序，本系统中未用
;----------------------------------------------------------
; 系统中断初始化
;----------------------------------------------------------
;事件管理器
POINT_EV
; SPLK #0000001000000000b,IMRA ;Enable T1 Underflow Int (i.e. Period)
SPLK #0000001000000001b,IMRA ;Enable T1 Underflow Int (i.e. Period)
; SPLK #0000000000000100b,IMRB ;Enable T2 Underflow Int (i.e. Period)
SPLK #0000000000000100b,IMRC ;Enable CAP3 int (i.e. QEP index pulse)
;||||!!!!||||!!!!
;5432109876543210
SPLK #0FFFFh,IFRA ; 清所有 A 组中断标志
SPLK #0FFFFh,IFRB ; 清所有 B 组中断标志
SPLK #0FFFFh,IFRC ; 清所有 C 组中断标志
;C2xx Core
POINT_PG0
;---实时中断选项 --------------------------------------------------
.if (real_time)
SPLK #0000000001001010b,IMR ;En Int lvl 2,4,7 (T1 ISR, CAP3 ISR)
.endif
.if (real_time != 1)
SPLK #0000000000001010b,IMR ;En Int lvl 2,4 (T1 ISR, CAP3 ISR)
; SPLK #0000000000000100b,IMR ;En Int lvl 3 (T2 ISR)
;||||!!!!||||!!!!
;5432109876543210
.endif
SPLK #0FFFFh, IFR ;清所用未决中断
EINT ;使能中断
;-----------------------------------------------------------------------
;使能 PWM 信号输出（本系统中未使用，techv cpu板可通过转接板指示灯观测）
;-----------------------------------------------------------------------
.if (x243|x2407) ;目标板
POINT_PF2
LACC OCRA
AND #0BFFFh
SACL OCRA ;选择 IOPB6 第二功能
LACC PBDATDIR
OR #04000h
SACL PBDATDIR ;Set IOPB6 as output
LACC PBDATDIR
AND #0FFBFh ;Set IOPB6 low, Enable PWM
; OR #00040h ;Set IOPB6 high, Disable PWM
SACL PBDATDIR
.endif
;---------------------------------------------------------
;系统初始化程序编译选项
;---------------------------------------------------------
POINT_B0
;---------------------------------------------------------
.if (step1) ;测试步骤一初始化程序代码
;---------------------------------------------------------
SPLK #01000h, speed_ref ; 转速参考值初始化Q15
SPLK #00800h, Vd_testing ; d轴电压分量初始化Q15
SPLK #00000h, Vq_testing ; q轴电压分量初始化Q15
SPLK #0,pdpint_flg ; 复位 PDPINT_FLG
LDP #step_angle_max ; 开环磁链发生模块初始化
SPLK #1638,step_angle_max ; 对应 fbase=250Hz, fs=10kHz
LDP #rmp_dly_max ; 磁链控制模块初始化
SPLK #120,rmp_dly_max ; 延迟周期数初始化
SPLK #8000h,rmp_lo_limit ; 输出下限
SPLK #7FFFh,rmp_hi_limit ; 输出上限
.endif
;---------------------------------------------------------
.if (step2) ;测试步骤二初始化程序代码
;---------------------------------------------------------
SPLK #01000h, speed_ref ; 转速参考值初始化Q15
SPLK #01000h, Vd_testing ; d轴电压分量初始化Q15
SPLK #01000h, Vq_testing ; q轴电压分量初始化Q15
SPLK #0,pdpint_flg ; 复位 PDPINT_FLG
LDP #step_angle_max ; 磁链发生模块初始化
SPLK #1638,step_angle_max ; 对应 fbase=250Hz, fs=10kHz
LDP #cal_angle ; QEP模块初始化
SPLK #350,cal_angle ; 根据编码器连接情况确定
LDP #rmp_dly_max ; 磁链控制模块初始化
SPLK #120,rmp_dly_max ; 延迟周期数初始化
SPLK #8000h,rmp_lo_limit ; 输出下限
SPLK #7FFFh,rmp_hi_limit ; 输出上限
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (step3) ;测试步骤三初始化程序代码
;---------------------------------------------------------
SPLK #01000h, speed_ref ; 转速参考值初始化Q15
SPLK #0,pdpint_flg ; 复位 PDPINT_FLG
SPLK #0000h, Id_ref ; 励磁电流参考值初始化Q15
SPLK #1000h, Iq_ref ; 转矩电流参考值初始化Q15
LDP #step_angle_max ; 磁链发生模块初始化
SPLK #1638,step_angle_max ; 对应 fbase=250Hz, fs=10kHz
LDP #rmp_dly_max ; 磁链控制模块初始化
SPLK #120,rmp_dly_max ; 延迟周期数初始化
SPLK #8000h,rmp_lo_limit ; 输出下限
SPLK #7FFFh,rmp_hi_limit ; 输出上限
.endif
;---------------------------------------------------------
;---------------------------------------------------------
.if (step4) ;测试步骤四初始化程序代码
;---------------------------------------------------------
SPLK #01000h, speed_ref ; 速度参考值初始化Q15
SPLK #0,lockrtr_flg ; lockrtr_flg初始化
SPLK #0,pdpint_flg ; pdpint_flg初始化
SPLK #0000h, Id_ref ; 励磁电流参考值初始化Q15
SPLK #1000h, Iq_ref ; 转矩电流参考值初始化Q15
LDP #step_angle_max ; 磁链发生模块初始化
SPLK #1638,step_angle_max ; 对应 fbase=250Hz, fs=10kHz
LDP #cal_angle ; QEP模块初始化
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -