;	File : PID.asm
;
;	Proportional, Integral, Differential control loop utility.

	GLOBAL	doPID, InitPID, SetTarget
	GLOBAL	K1,K2,K3,Target,DeltaV

#include	<P16C745.inc>

	errorlevel  -302              ; suppress message 302 from list file
	
#define MaximumTarget	H'C2'
#define TargetADC	MaximumTarget

#define subwl	sublw		; Ward's Secret Macro

Kp	equ	D'200'	 	; Proportional error multiplier
Ki 	equ	D'128'		; Integral error multiplier
Kd	equ	D'32'		; Differential error multiplier

;*****************************************************************************************
;*
;* Define RAM variables
;*

bank0			udata

multcnd		res	1	; 8 bit multiplicand
multplr		res	1	; 8 bit multiplier
xCount		res	1	; counter workspace used by Multiply and Tacq
H_Byte		res	1	; High byte of the 16 bit multiply result
L_Byte		res	1	; Low byte of the 16 bit multiply result
K1		res	1	; P multiplier
K2		res	1	; I multiplier
K3		res	1	; D multiplier
Target		res	1	; Target boost voltage ADC
MaxTarget	res	1	; Maximum target boost voltage
DeltaV		res	1	; difference between target and measured output voltage
LastDeltaV 	res	1	; previous DeltaV
H_Integral 	res	1	; ms byte of 16 bit error integrator
L_Integral 	res	1	; ls byte of 16 bit error integrator
H_PIDSum 	res	1	; ms byte of 16 bit PID sum
L_PIDSum 	res	1	; ls byte of 16 bit PID sum
PIDTemp		res	1	; temporary work space

	global	K1
	global	K2
	global	K3

PORT1	code

doPID
	; use value passed through W as latest input for PID

	movwf	PIDTemp

CtrlLoop	
	banksel	DeltaV
	movf	DeltaV,w			; get most recent error voltage
	movwf	LastDeltaV		; save as last

	bcf	STATUS,C			; assure zero in msb after rotate
	rrf	PIDTemp,w		; get latest input (7 bit signed)
	subwf	Target,w			; compute error voltage
	movwf	DeltaV			; save

; Proportional error

	movf	K1,w				; prepare for multiply by pre-placing...
	movwf	multplr			; ...Proportional multiplier and...
	movf	DeltaV,w			; ...error voltage...
	movwf	multcnd			; ...in multiply workspace
	call	Multiply			; multiply Proportional error by K1
	movf	H_Byte,w			; accumulate first of PID offsets
	movwf	H_PIDSum			;
	movf	L_Byte,w			;
	movwf	L_PIDSum			;

; Integral error

	movf	K2,w				; prepare for multiply by pre-placing...
	movwf	multplr			; ...Integral multiplier
	movf	DeltaV,w			; compute Integral Deltas
	movwf	multcnd			; save integral in multiply workspace
	call	Multiply			; multiply Integral error by K2
	call	Integrate		; integrate and accumulate

; Differential error

	movf	K3,w				; prepare for multiply by pre-placing...
	movwf	multplr			; ...Differential multiplier
	movf	LastDeltaV,w			; compute Differential voltage...
	subwf	DeltaV,w	; ...Differential = Delta - LastDelta
	movwf	multcnd			; save Differential in multiply workspace
	call	Multiply			; multiply Diffential error by K3
	call	Accumulate		; add to previous result

; return corrected control byte

	movf	H_PIDSum,w	; recover control byte into W
	return				; return the byte
	

Multiply
;*****************************************************************
;
;       8 x 8 multiplier routine
;       enter with two 8 bit numbers in multcnd and multplr
;       16 bit result is returned in H_Byte and L_Byte
;       multplr is altered in the process.
;	This is a semi-signed multiply which means that if the
;	multcnd is negative the result will be negative. The
;	multplr is always unsigned.
;
;*****************************************************************
;
	clrf    H_Byte		; clear workspace (L_Byte is self clearing)
	movlw   8		; multiply is 8 accumulate/rotates
	movwf   xCount		; set accumulate/rotate counter
	movf    multcnd,W	; restore multicand
	btfsc	multcnd,7	; is multicand negative?
	subwl	D'0'		; yes - make it positive by two's complimenting
MU10    
	rrf     multplr,f	; test multiplier LSB
	skpnc					; skip accumulate if LSB was zero
	addwf   H_Byte,f	; not zero - add multicand
	rrf     H_Byte,f	; right shift 16 bit result 1 place
	rrf     L_Byte,f	; 
	decfsz  xCount,f	; all 8 accumulate/rotates performed?
	goto    MU10		; no - repeat for next bit

	btfss	multcnd,7	; was multicand negative?
	return			; no - no further processing

	comf	L_Byte,f	; make positive result negative by two's complimenting
	comf	H_Byte,f	; invert all 16 bits...
	movlw	1		; ...and...
	addwf	L_Byte,f	; ...add 1
	skpnc				; adjust MS byte if carry from LS byte
	incf	H_Byte,f	;

	return

Integrate
;*****************************************************************
;
;	Sum the 16 bit result of K2 multiply result with
;	all previous results and add this integration to the PIDSum.
;
;	The integral is always positive but other addends are signed.
;       Determine overflow/underfow by comparing the msb's of the 
;       integral, addend, and addition result as follows:
;
;	Integral, Addend, Result - Conclusion - Action
;	    1       0       0      Overflow     Force Max
;	    0       1       1      Underflow    Force Min
;
;*****************************************************************

	movf	H_Integral,w	; get ms result
	movwf	PIDTemp		; save
	movf	H_Byte,w	; restore ms multiplication result
	addwf	H_Integral,f	; 16 bit accumulate	
	movf	L_Byte,w	; get ls result
	addwf	L_Integral,f	; 16 bit accumulate
	btfsc	STATUS,C	; lower byte addition carry?
	incf	H_Integral,f	; yes - adjust ms byte

; test for addition overflow

	btfss	PIDTemp,7	; test for pre-addition integral msb
	goto	IntegUnder	; zero - check for underflow
	
	btfsc	H_Byte,7	; is addend negative?
	goto	IGAccum		; yes - no overflow

	btfsc	H_Integral,7	; is result positive?
	goto	IGAccum		; no - no overflow
	
	movlw	0xff		; overflow detected...
	movwf	H_Integral	; ... force maximum
	movwf	L_Integral
	goto	IGAccum
	
; Test for addition underflow

IntegUnder
	btfss	H_Byte,7	; is addend positive?
	goto	IGAccum		; yes - no underflow

	btfss	H_Integral,7	; is result negative?
	goto	IGAccum		; no - no underflow
	
	clrf	H_Integral	; underflow detected...
	clrf	L_Integral	; ... force minimum

IGAccum	
; the integral is always positive 
;  accumulate integral and sum then check for over/under flow
;  the accumulation result must be positive

	movf	H_PIDSum,w	; get ms addend
	movwf	PIDTemp		; save
	movf	H_Integral,w	; restore ms integral
	addwf	H_PIDSum,f	; accumulate with sum
	movf	L_Integral,w	; restore ls byte of integral
	addwf	L_PIDSum,f	; sum with ls PIDSum
	btfsc	STATUS,C	; lower byte addition carry?
	incf	H_PIDSum,f	; yes - adjust ms byte

; test for addition overflow

	btfss	H_Integral,7	; test integral msb
	goto	IAUnder		; zero - check for underflow
	
	btfsc	PIDTemp,7	; is addend negative?
	goto	IntegExit	; yes - no overflow

	btfsc	H_PIDSum,7	; is result positive?
	goto	IntegExit	; no - no overflow
	
	movlw	0xff		; overflow detected...
	movwf	H_PIDSum	; ... force maximum
	movwf	L_PIDSum
	goto	IntegExit
	
; Test for addition underflow

IAUnder
	btfss	PIDTemp,7	; is addend positive?
	goto	IntegExit	; yes - no underflow

	btfss	H_PIDSum,7	; is result negative?
	goto	IntegExit	; no - no underflow
	
	clrf	H_PIDSum	; underflow detected...
	clrf	L_PIDSum	; ... force minimum

IntegExit
	return

Accumulate	
;*****************************************************************
;
;	Accumulate the 16 bit result of K3 PID multiply result
;
;	The sum accumulator (PIDSum) is assumed always positive
;
;*****************************************************************

	movf	H_PIDSum,w	; get pre-addition sum ms byte
	movwf	PIDTemp		; save
	movf	H_Byte,w	; get ms multiplication result
	addwf	H_PIDSum,f	; 16 bit accumulate
	movf	L_Byte,w	; get ls result
	addwf	L_PIDSum,f	; 16 bit accumulate
	btfsc	STATUS,C	; lower byte addition carry?
	incf	H_PIDSum,f	; yes - adjust ms byte
	
; test for accumulation overflow

	btfss	PIDTemp,7	; test pre-addition msb
	goto	ACUnder		; zero - check for underflow
	
	btfsc	H_Byte,7	; is addend negative?
	goto	AccumExit	; yes - no overflow

	btfsc	H_PIDSum,7	; is result positive?
	goto	AccumExit	; no - no overflow
	
	movlw	0xff		; overflow detected...
	movwf	H_PIDSum	; ... force maximum
	movwf	L_PIDSum
	goto	AccumExit
	
; Test for addition underflow

ACUnder
	btfss	H_Byte,7	; is addend positive?
	goto	AccumExit	; yes - no underflow

	btfss	H_PIDSum,7	; is result negative?
	goto	AccumExit	; no - no underflow
	
	clrf	H_PIDSum	; underflow detected...
	clrf	L_PIDSum	; ... force minimum

AccumExit
	return

InitPID
;************************************************************************************
;*
;* Initialize PID registers
;*
	movlw	Kp		; initialize PID multipliers
	movwf	K1
	movlw	Ki
	movwf	K2
	movlw	Kd
	movwf	K3
	clrf	DeltaV		; clear accumulators
	clrf	H_Integral
	clrf	L_Integral
	clrf	H_PIDSum
	clrf	L_PIDSum
	return

SetTarget
;************************************************************************
;*
;* take value passed in W and configure the target
;*

	movwf	Target		; set target
	bcf	STATUS,C	; clear carry for rotate
	rrf	Target,f	; convert to 7 bit signed
	
	return
	
	end