pi.s

基于DsPic30F4011的高精度位置伺服控制系统

;void CalcPI( tPIParm *pParm)
;{
;    Err  = InRef - InMeas
;    U  = Sum + Kp * Err
;    if( U > Outmax )
;        Out = Outmax
;    else if( U < Outmin )
;        Out = Outmin
;    else        
;        Out = U 
;    Exc = U - Out
;    Sum = Sum + Ki * Err - Kc * Exc
;}
;
;
;void InitPI( tPIParm *pParm)
;{
;    Sum = 0
;    Out = 0
;}
;*******************************************************************
;

	.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	;
	
	.equ	ErrW4,		w4
	.equ	WorkW5,		w5
	.equ	UnlimitW6,	w6
	.equ    WorkW7,		w7

;=================== CODE =====================	

	.section .text
	
	.global		_InitPI
	.global		InitPI
_InitPI:
InitPI:
	mov.w	w1,[BaseW0+PI_qOut]
	return
	
	.global		_CalcPosPI
	.global		CalcPosPI
_CalcPosPI:
CalcPosPI:	
	;;Err = InRef -InMeas
		
		;mov.w	[BaseW0 + PI_qInRef],WorkW7
		;mov.w	[BaseW0 + PI_qInMeas],WorkW5
		;sub.w	WorkW7,WorkW5,ErrW4
		mov.w	[BaseW0 + PI_qInErr],ErrW4
	;;U = Sum + Kp * Err * 2^NK0
		lac		[++BaseW0],B
		mov.w	[--BaseW0],WorkW5
		mov.w	WorkW5,ACCBL				;B = Sum
		
		mov.w  	[BaseW0 + PI_qKp],WorkW5
		mpy		ErrW4 * WorkW5,A			;Kp * Err
		sftac	A,#-NK0						;Kp * Err * 2^NK0
		add		A							;Sum + Kp * Err * 2^NK0
		sac		A,UnlimitW6					;UnlimitW6 = U	
	
	;;if( U > Outmax)
	;;	Out = Outmax
	;;else if(U < Outmin)
	;;	Out = Outmin	
	;;else
	;;	Out = U	
		mov.w	[BaseW0 + PI_qOutMax],OutW1
		cp		UnlimitW6,OutW1
		bra		GT,jPI5				;U > Outmax; OutW1 = Outmax
		
		mov.w	[BaseW0 + PI_qOutMin],OutW1
		cp		UnlimitW6,OutW1
		bra   	LE,jPI5				;U < Outmin; OutW1 = Outmin
		
		mov.w	UnlimitW6,OutW1
		
jPI5:
		mov.w	OutW1,[BaseW0 + PI_qOut]		

	;;Ki *Err
		mov.w	[BaseW0 + PI_qKi],WorkW5
		mpy		ErrW4*WorkW5,A

	;;Exc = U - Out
		sub.w	UnlimitW6,OutW1,UnlimitW6
		
	;;Ki * Err - Kc *Exc
		mov.w 	[BaseW0 + PI_qKc],WorkW5
		msc		WorkW5*UnlimitW6,A
	
	;;Sum = Sum + Ki * Err - Kc * Exc
		add 	A							;A+B->A
		
		sac		A,[++BaseW0]
		mov.w	ACCAL,WorkW5
		mov.w	WorkW5,[--BaseW0]
		
		return

		
	.global		_CalcPosP
	.global		CalcPosP		
_CalcPosP:		
CalcPosP:		
	
	
		mov.w	[BaseW0 + PI_qInErr],ErrW4
		;mov.w	[BaseW0 + PI_qInErr_1],WorkW5
		;mov.w	ErrW4,[BaseW0 + PI_qInErr_1]
		
	;U = Kp * Err * 2^NK0 + K1_p * Err_1 * 2^NK0
		mov.w	[BaseW0 + PI_qKp],WorkW5
		mpy		ErrW4*WorkW5,A
		
		mov.w	[BaseW0 + PI_qInErr_1],ErrW4
		mov.w	[BaseW0 + PI_qK1_p],WorkW5
		mac		ErrW4*WorkW5,A
		
			
		
	;;Kp*(Ek - Ek_1)	
		mov.w	[BaseW0 + PI_qInErr],WorkW7
		mov.w	[BaseW0 + PI_qInErr_1],WorkW5
		sub.w	WorkW7,WorkW5,ErrW4	
		mov.w	[BaseW0 + PI_qKp],WorkW5
		mac		ErrW4*WorkW5,A
		sftac	A,#-NK0	
	
		sac		A,UnlimitW6
		;mov.w   OutW1,[BaseW0 + PI_qOut]
	;;Ek_1 = Ek	
		mov.w	WorkW7,[BaseW0 + PI_qInErr_1]
		
	;;if( U > Outmax)
	;;	Out = Outmax
	;;else if(U < Outmin)
	;;	Out = Outmin	
	;;else
	;;	Out = U	
		mov.w	[BaseW0 + PI_qOutMax],OutW1
		cp		UnlimitW6,OutW1
		bra		GT,jincPI5				;U > Outmax; OutW1 = Outmax
		
		;;mov.w	[BaseW0 + PI_qOutMin],OutW1
		;;cp		UnlimitW6,OutW1
		;;bra   	LE,jPI5				;U < Outmin; OutW1 = Outmin
		
		mov.w	UnlimitW6,OutW1	
		
jincPI5:
		mov.w	OutW1,[BaseW0 + PI_qOut]
		
		
		return
		.end