drv_motor.asm

uCOS2 for 51系列

;/*********************************************************************************************************
;**				                         uC/OS 2 FOR TI TMS320240X
;**                                   The Real-Time Kernel(For CCS2.2)
;**
;**                                  
;**                                                V0.00.1
;**
;**
;**--------------文件信息--------------------------------------------------------------------------------
;**文   件   名: Drv_Motor.C
;**创   建   人: 龚树强
;**版        本: V0.01.3
;**最后修改日期: 2005年4月24日
;**描        述: 直流电机控制函数源文件
;**
;**--------------历史版本信息----------------------------------------------------------------------------
;** 创建人: 龚树强
;** 版  本: V0.01.3
;** 日　期: 2005年4月24日
;** 描　述: 原始版本
;**
;**------------------------------------------------------------------------------------------------------
;** 修改人: 
;** 版  本: 
;** 日　期: 
;** 描　述: 
;**
;**------------------------------------------------------------------------------------------------------
;********************************************************************************************************/
	.text
	
	.include "..\target\LF2407REGS.INC"

	.def 	_I_Modulate
	.def	_N_Modulate
	
	.ref	_N_KP					;速度调节比例系数
	.ref	_N_KI                	;速度调节积分系数
	.ref	_N_KC              		;速度调节积分饱和修正系数
	.ref	_N_REF               	;速度参考值
	.ref	_DIRECTION         		;速度方向标志
	.ref	_ENCODEROLD      		;前一次编码器读数
	.ref	_ENCINCR           		;编码增量
	.ref	_SPEEDTMP          		;编码脉冲累计
	.ref	_SPEEDSTEP          	;速度调节计数器
	.ref	_KSPEED             	;编码脉冲-速度转换系数
	.ref	_N                   	;速度反馈量
	.ref	_N_EK                	;速度偏差
	.ref	_N_RK                	;速度第K次积分调节累计
	.ref	_N_U                 	;速度调节输出
	.ref	_N_ELPI              	;速度极限偏差


	.ref	_I_KP              		;电流调节比例系数
	.ref	_I_KI                	;电流调节积分系数
	.ref	_I_KC             		;电流调节积分饱和修正系数
	.ref	_I_REF               	;电流参考值

	.ref	_I_EK              		;电流偏差
	.ref	_I_RK              		;电流第K次积分调节累计
	.ref	_I_U               		;电流调节输出	
	.ref	_I_ELPI            		;电流极限偏差
	.ref	_I_MIN               	;电流最小值
	.ref	_I_MAX             		;电流最大值


_I_Modulate:
	LACC	RESULT0,10         		;读ADC转换结果
	LDP 	#0
	SUB		_I_REF,16            	;减电流参考值,计算电流偏差
	NEG
	SACL	_I_EK               	;保存偏差
	LACC	_I_RK,12            	;左移12位
	LT  	_I_EK 
	MPY 	_I_KP               	;Q12格式
	APAC  
	SACH  	_I_U,4               	;相当于右移12位
	BIT 	_I_U,0               	;检测调节器输出的正负
	BCND 	UP,NTC            		;如果正,跳转
	LACC 	#0                  	;否则是负
	B  		LIMITERS
	
UP: 
	BIT    	_DIRECTION,15        	;检测转向
	BCND 	CW,TC               	;正转,跳转
	LACC 	#500                	;否则反转.检测是否超过上限
	SUB 	_I_U 
	BCND 	TOP,LT              	;超过上限进入饱和区则跳转
	LACC 	#250                 	;否则检测下限
	SUB  	_I_U
	BCND 	LOW,GT             		;超过下限跳转
	LACC 	_I_U                  	;否则正常调整
	B  		LIMITERS
	
TOP:   
	LACC 	#500                 	;ACC =上限值
	B 		LIMITERS
	
LOW: 
	LACC 	#250
	B  		LIMITERS
	
CW:  
	LACC  	#250                	;检测是否超过上限
	SUB 	_I_U
	BCND	CWTOP,LT          		;超过上限跳转
	LACC  	_I_U                	;否则正常调整
	B    	LIMITERS
	
CWTOP:
	LACC  	#250                	;ACC=上限值
	
LIMITERS:  
	LDP  	#0E8H
	SACL 	_CMPR2               	;输出,更新占空比
	SACL 	_CMPR1               	;更新占空比
	LDP   	#0
	SUB  	_I_U
	SACL 	_I_ELPI               	;求极限偏差
	LT  	_I_ELPI
	MPY 	_I_KC                	;Q12格式
	PAC 
	LT  	_I_EK
	MPY 	_I_KI                	;Q12格式
	APAC  
	ADD  	_I_RK,12
	SACH 	_I_RK,4              	;更新
;-----------------------读编码器脉冲数,计算转角增量和转速-----------------------------------------
	LDP  	#0E8H
	LACC  	T2CNT                	;读编码脉冲
	LDP  	#0
	SACL 	TMP
	SUB  	ENCODEROLD          	;减去前一个周期测的脉冲数
	SACL 	ENCINCR              	;得到编码增量
	ADD  	SPEEDTMP 
	SACL 	SPEEDTMP            	;编码脉冲累计
	LACC	TMP                    
	SACL 	ENCODEROLD         		;更新ENCODEROLD
	LACC 	SPEEDSTEP            	;检测是否该速度调节
	SUB  	#1 
	SACL	SPEEDSTEP
	BCND 	GO,GT                 	;没到,退出
	LT   	SPEEDTMP              	;否则计算转速
	MPY 	KSPEED               	;Q22格式
	PAC
	SACH 	N,6                    	;相当于右移10位,Q12格式
	LT 		N
	MPY  	#3000
	PAC
	SACH 	N，4                   	;速度反馈值,Q0格式
	LACC 	#0 
	SACL 	SPEEDTMP             	;SPEEDTMP清零
	SPLK  	#100, SPEEDSTEP       	; SPEEDSTEP重新赋初值
;--------------------------------------速度PI调节-----------------------------------------------------------
	LACC 	N_REF                	;速度给定值,由外部输入
	SUB   	N                    	;减反馈值
	SACL 	N_EK                 	;保存偏差
	LACC 	N_RK,12               	;左移12位
	LT  	N_EK 
	MPY 	N_KP                 	;比例系数,Q12格式
	APAC 
	SACH  	N_U,4                	;相当于右移12位
	LACC  	N_U                  	;检测调节器输出是否超限
	SUB   	I_MIN               	;与下限比较
	BCND 	POS1,GT              	;如果大于则跳转
	LACC 	I_MIN               	;否则超过下限 
	B    	LIM                  	;ACC =下限值
	
POS1:
	LACC  	N_U
	SUB  	I_MAX               	;与上限比较
	BCND  	POS2,GT             	;超过上限则跳转
	LACC 	N_U                 	;否则正常调整
	B   	LIM
	
POS2:
	LACC 	I_MAX               	; ACC =上限值
	
LIM:
	SACL 	I_REF                	;输出
	SUB   	N_U
	SACL  	N_ELPI              	;求极限偏差
	LT    	N_ELPI
	MPY   	N_KC                	;积分饱和修正系数,Q12格式
	PAC 
	LT 		N_EK
	MPY  	N_KI                	;积分系数,Q12格式
	APAC 
	ADD  	N_RK,12 
	SACH  	N_RK,4              	;更新N_RK


_N_Modulate:
	

	.end