;===================================================== ; 等效的 C代码 ;{

; register short Pos, Delta; ;

; Pos = POSCNT; ;

; Delta = Pos - EncoderParm.iPrevCnt; ; EncoderParm.iPrevCnt = Pos; ;

; if( iDelta >= 0 ) ;{

; // Delta > 0因为 ; // 1) vel > 0或

; // 2) Vel < 0且编码器计数值出现翻转返回。 ;

; if( Delta >= EncoderParm.iCntsPerRev/2 ) ;{

; // Delta >= EncoderParm.iCntsPerRev/2 => 负转速，出现翻转返回 ;

; Delta -= EncoderParm.iCntsPerRev; ;} ;} ; else ;{

; // Delta < 0因为 ; // 1) vel < 0或

; // 2) Vel > 0并出现翻转返回 ;;

; if( Delta < -EncoderParm.iCntsPerRev/2 ) ;{

; // Delta < -EncoderParm.iCntsPerRev/2 => 正转速，出现翻转返回 ;

; Delta += EncoderParm.iCntsPerRev; ;} ;} ;

; EncoderParm.iAccumCnt += Delta; ;

; EncoderParm.iVelCntDwn--; ; if(EncoderParm.iVelCntDwn) ; return; ;

; iVelCntDwn = iIrpPerCalc;

; qVelMech = qKvel * iAccumCnt * 2^Nvel; ; EncoderParm.iAccumCnt = 0; ;}

;=================== 代码 ===================== ; CalcVelIrp的寄存器使用

.equ PosW, w0 ; 当前位置： POSCNT .equ WorkW, w4 ; 工作寄存器

.equ DeltaW, w6 ; NewCnt - PrevCnt .global _CalcVelIrp .global CalcVelIrp _CalcVelIrp: CalcVelIrp:

;; 保存寄存器内容 push w0 push w4 push w6

;; Pos = uTestPos; .ifdef SIMU

mov.w _uTestPos,PosW ; 编码器值 ?? .else

mov.w POSCNT,PosW ; 编码器值 .endif

mov.w _EncoderParm+Encod_iPrevCnt,WorkW

;; 用新的 cnt更新前一个 cnt。

mov.w PosW,_EncoderParm+Encod_iPrevCnt

;; 计算 Delta = New - Prev sub.w PosW,WorkW,DeltaW bra N,jEncoder5 ; Delta < 0

;; Delta > 0因为

;; 1) vel > 0或