AN908
DS00908A-page 46
2004 Microchip Technology Inc.
;=================== CODE =====================
; Register usage for CalcVelIrp
.equ PosW,
w0
; current position: POSCNT
.equ WorkW,
w4
; Working register
.equ DeltaW,
w6
; NewCnt - PrevCnt
.global _CalcVelIrp
.global CalcVelIrp
_CalcVelIrp:
CalcVelIrp:
;; Save registers
push
w0
push
w4
push
w6
;; Pos = uTestPos;
.ifdef SIMU
mov.w _uTestPos,PosW
; encoder value ??
.else
mov.w POSCNT,PosW
; encoder value
.endif
mov.w _EncoEncod_iPrevCnt,WorkW
;; Update previous cnt with new cnt
mov.w PosW,_EncoEncod_iPrevCnt
;; Calc Delta = New - Prev
sub.w PosW,WorkW,DeltaW
bra N,jEncoder5
; Delta < 0
;; Delta > 0 either because
;;
1) vel is > 0 or
;;
2) Vel < 0 and wrapped around
lsr.w _EncoEncod_iCntsPerRev,WREG
; WREG = CntsPerRev/2
;; Is Delta < CntsPerRev/2
sub.w DeltaW,w0,WorkW
; Delta-CntsPerRev/2
bra N,jEncoder20
; 0 < Delta < CntsPerRev/2, Vel > 0
;; Delta >= CntsPerRev/2 => Neg speed, wrapped around
;; Delta = Delta - CntsPerRev
mov.w _EncoEncod_iCntsPerRev,w0
sub.w DeltaW,w0,DeltaW
;; Delta < 0, Vel < 0
bra jEncoder20
jEncoder5:
;; Delta < 0 either because
;; 1) vel is < 0 or
;; 2) Vel > 0 and wrapped around
lsr.w _EncoEncod_iCntsPerRev,WREG ; WREG = CntsPerRev/2
;; Is Delta + CntsPerRev/2 < 0
add.w DeltaW,w0,WorkW ; Delta+CntsPerRev/2
bra NN,jEncoder20 ; -CntsPerRev/2 <= Delta < 0, Vel > 0
