;******************************************************************
;* BENCHMARK: PID routine for 80296SA  (State Time = 40nS)
;* DATE: 09-012-1996
;* BY: Navin Govind
;* Intel Corporation
;******************************************************************
$include(_SFR_INC_)
; Perform PID calculations using the following equation

; The PID algorithm must be converted to discrete values for
; implementing the algorithm on a microprocessor
; The rectangular approximation is given by: 
; y(n) = y(n-1) + G1*e(n) + G2*e(n-1) + G3*e(n-2) + G4*e(n-3)


	 RSEG  AT  040h

;Working register declarations

out:		dsw	2
out_l		equ	out
out_h		equ	out+2


temp:		dsw	2
temp_l		equ	temp
temp_h		equ	temp+2


EN:	dsw	1	; 
ENM1:	dsw	1	; Previous error sample	
ENM2:	dsw	1	; Latest error sample
ENM3:	dsw	1	; Oldest error sample

G1:    	dsw   	1	; Constant for gain
G2:    	dsw   	1	; constant for gain
G3:    	dsw   	1	; Constant for gain
G4:    	dsw   	1	; Constant for gain

;NOTE: The fraction notational bit in the respective
;      SFR (special function register) must be set so
;      that a shift left (shll) need not be done after 
;      every multiply instruction. If the afore mentioned
;      SFR bit is not set a 'shll' instruction must follow
;      a 3-op multiply to preserve data coherency.


;MCS(r) 96 family of microcontrollers reset after power-up at 2080h
	CSEG AT 0FF2080H
	ejmp	START

;The following 'clr' instructions may not be needed for 80926SA
;if the contents of temp and out regs are not read initially

START:
	ld	sp,#STACK		;set up stack depth
	ldb	wsr,#1Eh		;set 128 byte window

	clr   	temp_l			;initialize temp register  
	clr   	temp_h				    	


;++++++++++++++++++++++++++++ PID VARIABLES ++++++++++++++++++++++++++++++++++++
	ld	out_l,#000ah
	ld	out_h,#000ch

	clr   	temp_l			;initialize temp register  
	clr   	temp_h				    	

	ld	ENM3,#000ah
	ld	ENM2,#000bh
	ld	ENM1,#000ch
	ld	EN,#000dh

	ld	G1,#000ah
	ld	G2,#000bh
	ld	G3,#000ch
	ld	G4,#000dh

;Kx and Nx implementation
;++++++++++++++++++++++++++++ PID ++++++++++++++++++++++++++++++++++++++++++++++
BEGIN:
	mul	temp,G1,EN	;multiply and accumulate 	16+4+4     
	add	out_l,temp_l
	addc	out_h,temp_h

	mul	temp,G2,ENM1
	add	out_l,temp_l
	addc	out_h,temp_h

	mul	temp,G3,ENM2
	add	out_l,temp_l
	addc	out_h,temp_h

	mul	temp,G4,ENM3
	add	out_l,temp_l
	addc	out_h,temp_h

;80296SA implementation
;++++++++++++++++++++++++++++ PID ++++++++++++++++++++++++++++++++++++++++++++++

	smacz	G1,EN		; Saturated multiply and accumulate 	2     
	smac	G2,ENM1		; Saturated multiply and accumulate	2     
	smac	G3,ENM2		; Saturated multiply and accumulate     2
	smac	G4,ENM3		; Saturated multiply and accumulate     2

;++++++++++++++++++++++++++++ PID ++++++++++++++++++++++++++++++++++++++++++++++
DONE:
	sjmp	DONE		; endless loop
  
  END