;===================================================== ; 等效的 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或