movff	deriv0,BARGB0		;YES, AARGB0:AARGB2 has result of Prop + Integ	
	movff	deriv1,BARGB1		;load derivative term
	movff	deriv2,BARGB2				
	
	movff	pid_stat1,temp_reg	;pid_stat1 ---> temp_reg				
	movlw	b'11000000'			;prepare for sign check of bits 7 & 6
	andwf	temp_reg,f,1
	
	movf	temp_reg,w,1			;check error sign & a_error sign bits
	sublw	0x00
	btfsc	STATUS,Z
	bra		add_neg_d			;bits 7 & 6 (00) are NEGATIVE, add them
	bra		other_combo_d		;bits 7 & 6 not equal to 00
add_neg_d			
	call	_24_BitAdd			;add negative sign values	
	bra		scale_down			;scale result
	
other_combo_d	
	movf	temp_reg,w,1
	sublw	0xC0
	btfsc	STATUS,Z
	bra		add_pos_d			;bits 7 & 6 (11) are POSITIVE, add them
	bra		find_mag_sub_d		;bits 7 & 6 (xx) are different signs , subtract them
add_pos_d
	call	_24_BitAdd			;add positive sign values
	bra		scale_down			;scale result
find_mag_sub_d
	call	mag_and_sub			;subtract unlike sign numbers	
	btfss	pid_stat1,mag,1		;which is greater in magnitude ?
	bra		deriv_mag			;BARGB is greater in magnitude
	bra		scale_down			;derivative term < part pid term, leave pid_sign as is
	
deriv_mag						;derivative term > part pid term
	bcf		pid_stat1,pid_sign,1	;PID result is negative
	btfsc	pid_stat1,d_err_sign,1			
	bsf		pid_stat1,pid_sign,1	;PID result is positive
		
scale_down
	clrf	BARGB0,1				;(Prop + Integ + Deriv) / 16 = FINAL PID RESULT to plant
	movlw	0x10	
	movwf	BARGB1,1
	call	FXD2416U
	movff	AARGB2,pid_out2		;final result ---> pid_out2
	movff	AARGB1,pid_out1		;final result ---> pid_out1
	movff	AARGB0,pid_out0		;final result ---> pid_out0
	
#ifdef		pid_100				;Final result needs to be scaled down to  0 - 100%
	movlw	0x01				;% ratio for propotional only
	movwf	BARGB0,1
	movlw	0x90
	movwf	BARGB1,1
	
	btfss	pid_stat2,integ_go,1	;if integral is included then change % conversion 
	bra		conv_percent		;no do proportional only	
	bcf		pid_stat2,integ_go,1	;clear integral go bit for next operation
	movlw	0x03				;% ratio for propotional & integral only
	movwf	BARGB0,1
	movlw	0x20
	movwf	BARGB1,1
	
	btfss	pid_stat2,deriv_go,1	;if derivative is included then change % conversion
	bra		conv_percent		;no do proportional & integral only	
	bcf		pid_stat2,deriv_go,1	;clear derivative go bit for next operation
	movlw	0x06				;% ratio for propotional & integral & derivative 
	movwf	BARGB0,1
	movlw	0x40
	movwf	BARGB1,1
	
conv_percent	
	call	FXD2416U			;pid_out0:pid_out2 / % ratio = 0 - 100% value	
	movf	AARGB2,W,1			;AARGB2 --> percent_out			
	movwf	percent_out,1		;error has been scaled down and is now available in a 0 -100% range
#endif	

	movff	error0,p_error0				;maintain previous error for derivative term		
	movff	error1,p_error1				;maintain previous error for derivative term
	bcf		pid_stat1,p_err_sign,1		;make p_error negative
	btfsc	pid_stat1,err_sign,1			;make p_error the same sign as error	
	bsf		pid_stat1,p_err_sign,1		;make p_error positive	
		
	bsf		PIE1,TMR1IE					;re-enable Timer 1 interrupt
	return								;return to mainline code

	
;--------- Find Accumulative Error for Integral, Zero? Limits? --------------------
get_a_error				
	movff	a_error0,BARGB0			;load error & a_error 
	movff	a_error1,BARGB1
	movff	a_error2,BARGB2
	movff	error0,AARGB1
	movff	error1,AARGB2
	
	call	spec_sign				;call routine for add/sub sign numbers	
	btfss	pid_stat1,mag,1			;which is greater in magnitude ?
	bra		a_err_zero				;bargb, keep sign as is or both are same sign
	
	bcf		pid_stat1,a_err_sign,1	;aargb, make sign same as error, a_error is negative
	btfsc	pid_stat1,err_sign,1			
	bsf		pid_stat1,a_err_sign,1	;a_error is positive	
	
a_err_zero
	bcf		pid_stat1,a_err_z,1		;clear a_error zero flag	
	movlw	0
	cpfseq	AARGB0,1					;is byte 0 = 00		
	bra		chk_a_err_limit			;NO, done checking
	
	cpfseq	AARGB1					;is byte 1 = 00				
	bra		chk_a_err_limit			;NO, done checking
	
	cpfseq	AARGB2,1					;is byte 2 = 00		
	bra		chk_a_err_limit			;NO, done checking
	bsf		pid_stat1,a_err_z,1		;YES, set zero flag
	movff	AARGB0,a_error0			;store the a_error	
	movff	AARGB1,a_error1	
	movff	AARGB2,a_error2	
	return							;a_error = 00, return 
	
chk_a_err_limit
	movff	AARGB0,a_error0			;store the a_error	
	movff	AARGB1,a_error1	
	movff	AARGB2,a_error2	
	
	movlw	0						;a_error reached limits?
	cpfseq	a_error0,1				;Is a_error0 > 0 ??, if yes limit has been exceeded
	bra		restore_limit			;YES, restore limit value
	
	cpfseq	a_error1,1				;Is a_error1 = 0 ??, if yes, limit not exceeded
	bra		chk_a_error1			;NO
	return							;YES
chk_a_error1	
	movlw	a_err_1_lim			
	cpfsgt	a_error1,1				;Is a_error1 > a_err_1_lim??
	return							;NO
	bra		restore_limit			;YES, restore limit value
chk_a_error2	
	movlw	a_err_2_lim			
	cpfsgt	a_error2,1				;Is a_error2 > a_err_2_lim ??
	return							;NO, return to mainline code	
	
restore_limit
	clrf	a_error0,1					;YES, a_error limit has been exceeded
	movlw	0x0F				
	movwf	a_error1,1		
	movlw	0xA0
	movwf	a_error2,1	
	return							;return to mainline code
	
	
;-------------- Find Delta Error for Derivative, Zero? --------------------
get_delta_error
	clrf	AARGB0,1					;load error and p_error
	movff	error0,AARGB1		
	movff	error1,AARGB2
	clrf	BARGB0,1
	movff	p_error0,BARGB1
	movff	p_error1,BARGB2
	
	movf	pid_stat1,w,1				;pid_stat1 ---> temp_reg
	movwf	temp_reg,1				;prepare for sign check of bits 4 & 2
	movlw	b'00010100'
	andwf	temp_reg,f,1
		
	movf	temp_reg,w,1				;check error sign & a_error sign bits	
	sublw	0x00
	btfsc	STATUS,Z		
	bra		p_err_neg				;bits 4 & 2 (00) are NEGATIVE, 
	bra		other_combo2			;bits 4 & 2 not equal to 00
p_err_neg
	call	mag_and_sub			
	bcf		pid_stat1,d_err_sign,1	;d_error is negative
	btfsc	pid_stat1,p_err_sign,1	;make d_error sign same as p_error sign
	bsf		pid_stat1,d_err_sign,1	;d_error is positive
	bra		d_error_zero_chk		;check if d_error = 0

other_combo2	
	movf	temp_reg,w,1
	sublw	0x14
	btfsc	STATUS,Z
	bra		p_err_pos				;bits 4 & 2 (11) are POSITIVE
	bra 	p_err_add				;bits 4 & 2 (xx) are different signs
	
p_err_pos
	call	mag_and_sub
	bcf		pid_stat1,d_err_sign,1	;d_error is negative
	btfsc	pid_stat1,p_err_sign,1	;make d_error sign same as p_error sign
	bsf		pid_stat1,d_err_sign,1	;d_error is positive
	bra		d_error_zero_chk		;check if d_error = 0
p_err_add
	call	_24_BitAdd				;errors are different sign	
	bcf		pid_stat1,d_err_sign,1	;d_error is negative
	btfsc	pid_stat1,err_sign,1		;make d_error sign same as error sign
	bsf		pid_stat1,d_err_sign,1	;d_error is positive

d_error_zero_chk
	movff	AARGB1,d_error0	
	movff	AARGB2,d_error1	
	bcf		pid_stat2,d_err_z,1
	
	movlw	0
	cpfseq	d_error0,1			;is d_error0 = 00		
	return						;NO, done checking
	
	cpfseq	d_error1,1			;YES, is d_error1 = 00		
	return						;NO, done checking
	bsf		pid_stat2,d_err_z,1	;set delta error zero bit	
	return						;YES, return to ISR
		
	
;---------------------- Special Sign Routine ---------------------------------
spec_sign
	movff	pid_stat1,temp_reg	;pid_stat1 ---> temp_reg				
	movlw	b'00001100'			;prepare for sign check of bits 3 & 2
	andwf	temp_reg,f,1
	
	movf	temp_reg,w,1			;check error sign & a_error sign bits
	sublw	0x00
	btfsc	STATUS,Z
	bra		add_neg				;bits 3 & 2 (00) are NEGATIVE, add them
	bra		other_combo			;bits 3 & 2 not equal to 00
add_neg			
	call	_24_BitAdd			;add negative sign values	
	return
	
other_combo	
	movf	temp_reg,w,1
	sublw	0x0C
	btfsc	STATUS,Z
	bra		add_pos				;bits 3 & 2 (11) are POSITIVE, add them
	bra		find_mag_sub		;bits 3 & 2 (xx) are different signs , subtract them
add_pos
	call	_24_BitAdd			;add positive sign values
	return
find_mag_sub
	call	mag_and_sub			;subtract unlike sign numbers	
	return
	
	
;------------------- Find Magnitude and Subtract -------------------------	
mag_and_sub			
	movf	BARGB0,w,1
	subwf	AARGB0,w,1			;AARGB0 - BARGB0 --> W
	btfsc	STATUS,Z			;= zero ?
	bra		check_1				;YES
	btfsc	STATUS,C			;borrow ?
	bra		aargb_big			;AARGB0 > BARGB0, no borrow			
	bra		bargb_big			;BARGB0 > AARGB0, borrow
check_1	
	movf	BARGB1,w,1
	subwf	AARGB1,w,1			;AARGB1 - BARGB1 --> W
	btfsc	STATUS,Z			;= zero ?
	bra		check_2				;YES
	btfsc	STATUS,C			;borrow ?
	bra		aargb_big			;AARGB1 > BARGB1, no borrow			
	bra		bargb_big			;BARGB1 > AARGB1, borrow
			
check_2
	movf	BARGB2,w,1			;AARGB2 - BARGB2 --> W
	subwf	AARGB2,w,1
	btfsc	STATUS,C			;borrow ?
	bra		aargb_big			;AARGB2 > BARGB2, no borrow		
	bra		bargb_big			;BARGB2 > AARGB2, borrow
	
aargb_big
	call	_24_bit_sub		
	bsf		pid_stat1,mag,1		;AARGB is greater in magnitude
	return
	
bargb_big
	movf	BARGB1,W,1			;swap AARGB1 with BARGB1
	movff	AARGB1,temp_reg
	movwf	AARGB1,1
	movff	temp_reg,BARGB1	
	movf	BARGB2,W,1			;swap AARGB2 with BARGB2
	movff	AARGB2,temp_reg
	movwf	AARGB2,1
	movff	temp_reg,BARGB2
	call	_24_bit_sub			;BARGB > AARGB	
	bcf		pid_stat1,mag,1		;BARGB is greater in magnitude					
	return	

;------------------- High Interrupt Routine -------------------------	
;highInt
;	btfss	PIR1,TMR1IF					;has T1 overflowed?
;	retfie								;NO, exit ISR

PID_INT
	GLOBAL	PID_INT		

	bcf		PIR1,TMR1IF					;YES, clear T1 interrupt flag

	movlw	timer1_hi					;reload T1 registers with 1ms count
	movwf	TMR1H
	movlw	timer1_lo
	movwf	TMR1L

	bsf		pid_stat2,timer_expire,1

	call	PID_INT_CALC		
	retfie	FAST							;YES
	
PID_INT_CALC

	bcf		pid_stat2,timer_expire,1
	
	btfsc	pid_stat1,err_z,1				;Is error = 00 ?
	return


	
	movwf	T_WREG,1
	movff	STATUS,T_STATUS
	movff	BSR,T_BSR					;if needed
	
	movff	AARGB0,T_AARGB0			;NO, context save Math varialbes
	movff	AARGB1,T_AARGB1
	movff	AARGB2,T_AARGB2
	movff	AARGB3,T_AARGB3
	movff	BARGB0,T_BARGB0
	movff	BARGB1,T_BARGB1
	movff	BARGB2,T_BARGB2
	movff	BARGB3,T_BARGB3
	movff	REMB0,T_REMBO
	movff	REMB1,T_REMB1
	movff	TEMP,T_TEMP
	movff	TEMPB0,T_TEMPB0
	movff	TEMPB1,T_TEMPB1
	movff	TEMPB2,T_TEMPB2
	movff	TEMPB3,T_TEMPB3
	movff	ZARGB0,T_ZARGB0
	movff	ZARGB1,T_ZARGB1
	movff	ZARGB2,T_ZARGB2
	movff	CARGB2,T_CARGB2
	movff	LOOPCOUNT,T_LOOPCOUNT
	movff	temp_reg,t_temp_reg
	
	btfsc	pid_stat1,a_err_z,1			;Is a_error = 0
	bra		derivative_ready?			;YES, check if derivative is ready
	bsf		pid_stat2,integ_go,1			;NO, set intergral go bit for PID calculation
	call	start_integral				;find integral
	
derivative_ready?
	decfsz	deriv_count,f,1 				;is it time for derivative term ?
	bra		skip_deriv					;NO, finish ISR
	call	start_deriv					;YES, find derivative

			;---Get ready for Next Derivative Term	
	movlw	deriv_cnt
	movwf	deriv_count,1					;derivative action = TMR1H:TMR1L * deriv_count

skip_deriv	
;	movlw	timer1_hi					;reload T1 registers with 1ms count
;	movwf	TMR1H
;	movlw	timer1_lo
;	movwf	TMR1L
	
	movf	T_WREG,w,1
	movff	T_STATUS,STATUS
	movff	T_BSR,BSR					;if needed
	
	movff	T_AARGB0,AARGB0				;restore Math variables
	movff	T_AARGB1,AARGB1
	movff	T_AARGB2,AARGB2
	movff	T_AARGB3,AARGB3
	movff	T_BARGB0,BARGB0
	movff	T_BARGB1,BARGB1
	movff	T_BARGB2,BARGB2
	movff	T_BARGB3,BARGB3
	movff	T_REMBO,REMB0
	movff	T_REMB1,REMB1
	movff	T_TEMP,TEMP
	movff	T_TEMPB0,TEMPB0
	movff	T_TEMPB1,TEMPB1
	movff	T_TEMPB2,TEMPB2
	movff	T_TEMPB3,TEMPB3
	movff	T_ZARGB0,ZARGB0
	movff	T_ZARGB1,ZARGB1
	movff	T_ZARGB2,ZARGB2
	movff	T_CARGB2,CARGB2
	movff	T_LOOPCOUNT,LOOPCOUNT
	movff	t_temp_reg,temp_reg
	
;	RETFIE			        			;return from interrupt	
	return
	
	END               					;directive 'end of program'