calcvel.s

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

;*******************************************************************
; Routines: InitCalcVel, CalcVel
;
;*******************************************************************
		.include "general.inc"
		.include "encoder.inc"
;*******************************************************************
; void InitCalcVel(void)
;         Initialize private velocity variables.  
;         iIrpPerCalc must be set on entry.
;*******************************************************************

		.equ Work0W,	w4	; Working register
		.equ PosW,		w5	; current position: POSCNT
		

		.global	_InitCalcVel
		.global	InitCalcVel
_InitCalcVel:
InitCalcVel:
	;;Disable interrupts for the next 5 instructions 
		DISI	#5
		
		mov.w	POSCNT,PosW		; current encoder value
		mov.w	PosW,_EncoderParm+Encod_iPrevCnt
		clr.w	_EncoderParm+Encod_iAccumCnt

	;;Load iVelCntDwn
		mov.w	_EncoderParm+Encod_iIrpPerCalc,WREG
		mov.w	WREG,_EncoderParm+Encod_iVelCntDwn
		return
;=====================================================
; Equivalent C code
;{
;    register short Pos, Delta; 
;
;    Pos = POSCNT;
;
;    Delta = Pos - EncoderParm.iPrevCnt;
;    EncoderParm.iPrevCnt = Pos;
;
;    if( iDelta >= 0 )
;        {
;        // Delta > 0 either because 
;        //       1) vel is > 0 or 
;        //       2) Vel < 0 and encoder wrapped around 
;
;        	if( Delta >=  EncoderParm.iCntsPerRev/2 )
;           	{
;            		// Delta >= EncoderParm.iCntsPerRev/2 => Neg speed, wrapped around
;
;            		Delta -= EncoderParm.iCntsPerRev;
;					EncoderParm.iIndexCnt --;				//电机反转，并发生计数翻转。索引值累减
;            	}
;        }
;    else
;        {
;        // Delta < 0 either because 
;        //       1) vel is < 0 or 
;        //       2) Vel > 0 and wrapped around
;
;        	if( Delta < -EncoderParm.iCntsPerRev/2 )
;            	{
;            		// Delta < -EncoderParm.iCntsPerRev/2 => Pos vel, wrapped around
;
;            		Delta += EncoderParm.iCntsPerRev;
;					EncoderParm.iIndexCnt ++;				//电机正转，并发生计数翻转。索引值累加
;            	}
;        }
;
;    EncoderParm.iAccumCnt += Delta;
;
;    EncoderParm.iVelCntDwn--;
;    if(EncoderParm.iVelCntDwn)
;        return;
;
;    iVelCntDwn = iIrpPerCalc; 
;    qVelMech = qKvel * iAccumCnt * 2^Nvel;
;    EncoderParm.iAccumCnt = 0;
;}
;=================== CODE =====================
; Register usage for CalcVelIrp 
		.equ PosW,		w0		;current position: POSCNT
		
		.equ PosCnt,	w2		;
		.equ WorkW,		w4		;
		.equ DeltaW,	w6		;

		.global		_CalcVelAngIrp
		.global		CalcVelAngIrp

_CalcVelAngIrp:
CalcVelAngIrp:

	;;Save registers
		push	w0
		push	w2
		push	w4
		push	w6

		mov.w	POSCNT,PosW			;encoder value
		mov.w	PosW,PosCnt			;
		
		mov.w	_EncoderParm+Encod_iPrevCnt,WorkW
	
	;;Update previous cnt with new cnt
		mov.w	PosW,_EncoderParm+Encod_iPrevCnt

	;;Calc Delta = New - Prev	
		sub.w	PosW,WorkW,DeltaW
		bra		N,jEncoder5			;Delta < 0 

	;; Delta >= 0 either because 
	;;		1)Vel is > 0 or
	;;		2)Vel < 0 and 发生翻转
		lsr.w	_EncoderParm+Encod_iMotorCntsPerRev,WREG	;WREG = CntsPerRev/2	
	;;	Is Delta < CntsPerRev/2
		sub.w	DeltaW,w0,WorkW		; Delta-CntsPerRev/2
		bra		N,jEncoder20 		; 0 < Delta < CntsPerRev/2, Vel > 0
	
	;;Delta >= CntsPerRev/2	=> Neg speed,
	;;Delta = Delta - CntsPerRev
	;;EncoderParm.iIndexCnt --
	;;if(iIndexCnt <= -40) 
	;;iIndexCnt = -40
		mov.w	_EncoderParm+Encod_iMotorCntsPerRev,w0
		sub.w	DeltaW,w0,DeltaW
		
		dec.w	_EncoderParm+Encod_iIndexCnt
		mov.w	#-40,w0
		cp.w	_EncoderParm+Encod_iIndexCnt
		bra		gt,jEncoder20
		mov.w	w0,_EncoderParm+Encod_iIndexCnt
		
	;;Delta < 0,Vel < 0
		bra		jEncoder20 

jEncoder5:	
	;; Delta < 0 either because 
	;;		1)Vel is < 0 or
	;;		2)Vel > 0 and 发生翻转
		lsr.w	_EncoderParm+Encod_iMotorCntsPerRev,WREG	;WREG = CntsPerRev/2
			
	;; Is Delta + CntsPerRev/2 < 0
		add.w	DeltaW,w0,WorkW		;Delta+CntsPerRev/2
		bra		NN,	jEncoder20		; -CntsPerRev/2 <= Delta < 0, Vel < 0	

	;;Delta < -CntsPerRev/2 => Pos Vel,发生翻转
	;;Delta = Delta + CntsPerRev
	;;EncoderParm.iIndexCnt ++;
	;;if(iIndexCnt >= 39)
	;;iIndexCnt = 39
		mov.w	_EncoderParm+Encod_iMotorCntsPerRev,w0
		add.w	DeltaW,w0,DeltaW
		
		inc.w	_EncoderParm+Encod_iIndexCnt
		mov.w	#39,w0
		cp.w	_EncoderParm+Encod_iIndexCnt
		bra		le,	jEncoder20
		mov.w	w0,_EncoderParm+Encod_iIndexCnt
		
jEncoder20 :
	
	;;	Delta现在包含位置的有符号变化

	;; EncoderParm.Delta += Delta;
		mov.w	DeltaW,w0
		add.w	_EncoderParm+Encod_iAccumCnt

	;; EncoderParm.iVelCntDwn--;
	;;if(EncoderParm.iVelCntDwn) return;
		
		dec.w	_EncoderParm+Encod_iVelCntDwn
		cp0.w	_EncoderParm+Encod_iVelCntDwn
		bra		NZ,jEncoder40
		
	;; Reload iVelCntDwn: iVelCntDwn = iIrpPerCalc; 	
		mov.w	_EncoderParm+Encod_iIrpPerCalc,WREG
		mov.w	WREG,_EncoderParm+Encod_iVelCntDwn
			
	;; Copy iAccumCnt to iDeltaCnt then iAccumCnt = 0
		mov.w	_EncoderParm+Encod_iAccumCnt,DeltaW
		mov.w	DeltaW,_EncoderParm+Encod_iDeltaCnt
		clr.w	_EncoderParm+Encod_iAccumCnt

jEncoder40:

	;;电机角度计算(1.15)	
	;;//qWheelMechAng = qKang_Indx * ( iIndexCnt / 2^15)
	;;qWheelMechAng = Index/39 + Poscnt/(4096*39)
	;;iWheelCnt为一个整数，但作为Q15，iWheelCnt = (iWheelCnt/2^15)
	
	;;if(iIndexCnt >= 39)
	;;{	iIndexCnt = 39;
	;;	POSCNT = 0;}
	;;else if(iIndexCnt <= -39)
	;;{	iIndexCnt = -39;
	;;	POSCNT = 0;}
		mov.w	#39,w0
		cp.w	_EncoderParm+Encod_iIndexCnt
		bra		le,	jMotorAng5
		mov.w	w0,_EncoderParm+Encod_iIndexCnt		;iIndexCnt = 39;
		mov.w	#0,PosCnt							;
		bra		jMotorAng10	
			
jMotorAng5:
		mov.w	#-40,w0
		cp.w	_EncoderParm+Encod_iIndexCnt
		bra		gt,jMotorAng10
		mov.w	w0,_EncoderParm+Encod_iIndexCnt	
		mov.w	#0,PosCnt
	
jMotorAng10:
		
		mov.w	_EncoderParm+Encod_iIndexCnt,DeltaW
		lac		DeltaW,A
		sftac	A,#-Nang_W
		sac		A,DeltaW			;将iIndexCnt扩大2^Nang_W
							
		mov.w	_EncoderParm+Encod_qKang_Indx,WorkW
		
		mpy		WorkW*DeltaW,A	
		
		sac		A,WorkW
		lac		WorkW,B
		mov.w	ACCAL,WorkW
		mov.w	WorkW,ACCBL		;A->B
		
		mov.w	PosCnt,DeltaW			;encoder value
		
		mov.w	_EncoderParm+Encod_qKang_Pos,WorkW
		mpy		WorkW*DeltaW,A			;
		
		add		A
		
		sac		A,#(Nang_W - 15),WorkW	; left shift by 15-Nang_W
		
		mov.w	WorkW,_EncoderParm+Encod_qWheelMechAng

		pop		w6
		pop		w4
		pop		w2
		pop		w0
		
		return


		.global	_CalcVel
		.global	CalcVel
_CalcVel:
CalcVel:
	;;qSteerMechAng = qKang_S * ( Delta / 2^Nvel / 2^15)

	;;iDeltaCnt为一个整数，但作为Q15，iDeltaCnt=(iDeltaCnt/2^15)
		mov.w	_EncoderParm+Encod_iDeltaCnt,DeltaW
		mov.w	_EncoderParm+Encod_qKvel,WorkW	

		mpy		WorkW*DeltaW,A				; dKvel * (Delta/2^15)
		sac		A,#(Nvel - 15),WorkW	; left shift by 15-Nvel
		
	;;qVelMech = qKvel * Q15(Delta / 2^Nvel)
		mov.w	WorkW,_EncoderParm+Encod_qMotorVelMech
		return



		.end