9.3.4 无刷直流电机速度闭环控制系统.txt

直流无刷电机(BLDC)的闭环控制系统汇编代码.

CALC_ACT_RPM
	movff	VELOCITY_READH,ARG2H        	//Timer5计数值高位导入
	movff	VELOCITY_READL,ARG2L		//Timer5计数值低位导入
	movlw	HIGH(RPM_CONSTANT_HALL)		//常数导入除法运算
	movwf	ARG1H
	movlw	LOW(RPM_CONSTANT_HALL)
	movwf	ARG1L
	call	DIVISION_15BY15          		//15位/15位除法运算
	movff	RESL,SPEED_FEEDBACKL		//运算结果为实际RPM值
	movff	RESH,SPEED_FEEDBACKH		//存入到SPEED_FEEDBACK
			
	return


CALCULATE_SPEED_ERROR
	movlw	LOW(SPEED_REF_RATIO)
	mulwf	SPEED_REFH                 	//读取速度给定值
	movff	PRODH,TEMP                		//从ADC读取速度给定值
	movlw	HIGH(SPEED_REF_RATIO)
	mulwf	SPEED_REFH					//在简化计算中读取8位
	movf	PRODL,W
	addwf	TEMP,F
	clrf		WREG
	addwfc	PRODH,W                    	//低8位被丢弃
	movwf	SPEED_REF_RPMH				//速度参考值被导入
	movff	TEMP,SPEED_REF_RPML			//速度给定值低8位导入
	RETURN



Speed Error = SPEED_REF_RPM - SPEED_FEEDBACK 	//计算速度偏差
	bsf		STATUS,C
	movf	SPEED_REF_RPML,W
	subfwb	SPEED_FEEDBACKL,W
	movwf	SPEED_ERRORL
	movf	SPEED_REF_RPMH,W
	subfwb	SPEED_FEEDBACKH,W
	movwf	SPEED_ERRORH
	bcf		FLAGS,NEGATIVE_ERROR		//Error是负值？
	bsf		pid_stat1,err_sign,1		//Error符号是正值
	btfss	SPEED_ERRORH,7            	//如果Error是负值补足Error
	bra		POSITIVE_ERROR
	comf	SPEED_ERRORH,F	
	comf	SPEED_ERRORL,F
	bsf		FLAGS,NEGATIVE_ERROR      	//设置Error标志位来指示负值Error
	bcf		pid_stat1,err_sign,1		//Error对于PID控制器是负值
POSITIVE_ERROR                				//根据速度偏差计算PWM占空比增量
Error PWM = Error_PWM_constant(8bit) * Error(15bit)	
	movlw	(ERROR_PWM_CONSTANT)		//计算PWM增量
	mulwf	SPEED_ERRORL
	movff	PRODH,TEMP
	movff	PRODL,ERROR_PWML
	movlw	(ERROR_PWM_CONSTANT)
	mulwf	SPEED_ERRORH
	movf	PRODL,W
	addwf	TEMP,W
	movwf	ERROR_PWMH
	Call		PID_ALGORITHM
RETURN