2004 Microchip Technology Inc.
DS00908A-page 63
AN908
;
.include "general.inc"
; External references
.include "PI.inc"
; Register usage
.equ BaseW0,
w0
; Base of parm structure
.equ OutW1,
w1
; Output
.equ SumLW2,
w2
; Integral sum
.equ SumHW3,
w3
; Integral sum
.equ ErrW4,
w4
; Error term: InRef-InMeas
.equ WorkW5,
w5
; Working register
.equ Unlimit
W6,w6
; U: unlimited output
.equ WorkW7,
w7
; Working register
;=================== CODE =====================
.section
.text
.global
_InitPI
.global
InitPI
_InitPI:
InitPI:
mov.w
w1,[PI_qOut]
return
.global
_CalcPI
.global
CalcPI
_CalcPI:
CalcPI:
;; Err = InRef - InMeas
mov.w
[PI_qInRef],WorkW7
mov.w
[PI_qInMeas],WorkW5
sub.w
WorkW7,WorkW5,ErrW4
;; U = Sum + Kp * Err * 2^NKo
lac
[++BaseW0],B
; AccB = Sum
mov.w
[--BaseW0],WorkW5
mov.w
WorkW5,ACCBLL
mov.w [PI_qKp],WorkW5
mpy ErrW4*WorkW5,A
sftac
A,#-NKo
; AccA = Kp*Err*2^NKo
add
A
; Sum = Sum + Kp*Err*2^NKo
sac
A,UnlimitW6
; store U before tests
;; if( U > Outmax )
;;
Out = Outmax
;; else if( U < Outmin )
;;
Out = Outmin
;; else
;; Out
=
U
mov.w [PI_qOutMax],OutW1
cp
UnlimitW6,OutW1
bra
GT,jPI5
; U > Outmax; OutW1 = Outmax
