calcang.s

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

;*******************************************************************
; Routines: InitCalcVel, CalcVel
;
;*******************************************************************
		.include "general.inc"
		.include "encoder.inc"

		.global	_InitCalcAng
		.global	InitCalcAng
_InitCalcAng:
InitCalcAng:
		

		return
		
;=====================================================
; Equivalent C code
;{
;	register unsigned int temp;
;	temp = PORTB & 0x0008;
;	temp |= (PORTC & 0x4000);
;	if(temp == 0x4008 || temp == 0)
;	{
;		EncoderParm.iSteerCnt ++;			//方向盘正转	
;		if(EncoderParm.iSteerCnt >= diEnMaxSteerCnts)
;			EncoderParm.iSteerCnt = diEnMaxSteerCnts;		
;	}
;	else if(temp == 0x0008 || temp == 0x4000)
;	{
;
;		EncoderParm.iSteerCnt --;			//方向盘反转
;		if(EncoderParm.iSteerCnt <= -diEnMaxSteerCnts)
;			EncoderParm.iSteerCnt = -diEnMaxSteerCnts;			
;	}	
;
;	return;
;}
;=================== CODE =====================
; Register usage for CalcVelIrp 

		.equ WorkW0,	w0		;current position: POSCNT
		.equ Qvalue,	w2		;
		.equ WorkW4,	w4		;



		.global		_CalcAngIrp
		.global		CalcAngIrp		
_CalcAngIrp:
CalcAngIrp:

		push	w0
		push	w2
		push	w4
	;;Qvalue = PORTB & 0x0008;
		mov.w	_PORTB,WorkW0
		and.w	WorkW0,#8,Qvalue
	;;Qvalue |= (PORTC & 0x4000);	
		mov.w	_PORTC,WorkW0
		mov.w	#0x4000,WorkW4
		and.w	WorkW0,WorkW4,WorkW0
		ior.w	WorkW0,Qvalue,Qvalue
	;;if(temp == 0x4008 || temp == 0)
		mov.w	#0x4008,WorkW0
		cp		Qvalue,WorkW0
		bra		z,JP_AngIrp5
		cp0.w	Qvalue	
		bra		nz,JP_AngIrp10
		
JP_AngIrp5:
	;;EncoderParm.iSteerCnt ++;	
		inc.w	_EncoderParm+Encod_iSteerCnt
	;;if(EncoderParm.iSteerCnt >= diEnMaxSteerCnts)	
		mov.w	#0x1ffe,WorkW0
		cp.w	_EncoderParm+Encod_iSteerCnt
		bra		le,JP_AngIrp15
		
	;;EncoderParm.iSteerCnt = diEnMaxSteerCnts;	
		mov.w	#0x1fff,WorkW0
		mov.w	WorkW0,_EncoderParm+Encod_iSteerCnt
		bra		JP_AngIrp15
JP_AngIrp10:		
	;;else if(temp == 0x0008 || temp == 0x4000)	
		mov.w	#8,WorkW0
		cp.w	Qvalue,WorkW0
		bra		z,JP_AngIrp12
		
		mov.w	#0x4000,WorkW0
		;sub.w	Qvalue,	WorkW0,Err
		cp.w	Qvalue,	WorkW0
		bra		nz,JP_AngIrp15
JP_AngIrp12:		
	;;EncoderParm.iSteerCnt --;			//方向盘反转
		dec.w	_EncoderParm+Encod_iSteerCnt
	;;if(EncoderParm.iSteerCnt <= -diEnMaxSteerCnts)
		mov.w	#0xe001,WorkW0
		cp.w	_EncoderParm+Encod_iSteerCnt
		bra		gt,JP_AngIrp15

		mov.w	#0xe001,WorkW0
		mov.w	_EncoderParm+Encod_iSteerCnt		

JP_AngIrp15:
		pop		w4
		pop		w2
		pop		w0
		
		return

;=================== CODE =====================
; Register usage for CalcVelIrp 
		.equ PosW,		w0		;current position: POSCNT
		
		.equ WorkW,		w4		;
		.equ WorkW5,	w5		;
		.equ DeltaW,	w6		;

		.global	_CalcAng
		.global	CalcAng
_CalcAng:
CalcAng:

	;;方向盘角度计算(1.15)	
	;;qSteerMechAng = qKang_S * ( Delta / 2^Nang_S / 2^15)
	;;iSteerCnt为一个整数，但作为Q15，iSteerCnt=(iSteerCnt/2^15)	
		mov.w	_EncoderParm+Encod_iSteerCnt,DeltaW
		mov.w	_EncoderParm+Encod_qKang_S,WorkW
		
		mpy		WorkW*DeltaW,A		

		sac		A,#(Nang_S - 15),WorkW	; left shift by 15-Nang_S
		
		mov.w	WorkW,_EncoderParm+Encod_qSteerMechAng
		

		
		
		return
		
		
		.end