penasm.asm

这个是我在microchip公司的网站下载的PIC18系列单片的一些驱动程序

	GOTO	MAIN				;DO_PID IS ZERO WAIT FOR IT TO CHANGE

PID_INT							;CALCULATE THE INTERGRAL TERM
								;INTEGRAL TERM IS KI*TS*[SUM(EN0)]
								;MULTIPLY EN0 BY KI AND THEN DIVIDE BY FS
								;THE MULTIPLY NEEDS TO BE DONE FIRST SO YOU DON'T LOOSE RESULUTION
								;SUM RESULT INTO THE INT_TERM
	BTFSC	INT_TERM_H,	7		;CHECK IF THE INTEGRAL TERM IS POSITIVE OR NEGATIVE
	GOTO	SUME_NEG
SUME_POS
	BTFSS	EN0, 7				;INTEGRAL TERM IS POSITIVE CHECK SIGN OF THE ERROR TERM
	GOTO	CHECK_2_BIG			;ERROR TERM IS ALSO POSITIVE, CHECK IF RESULT WILL BE TOO BIG
ADD_INT_TERM	
	MOVF	EN0,	W
	MOVWF	AARGB0				;MOVE EN0 TO THE MULTIPLICAND
	MOVF	KI,		W
	MOVWF	BARGB0				;MOVE KI TO THE MULTIPLIER
	CALL	FXM0808S			;CALL THE MULTIPLY SUBROUTINE 16 BIT RESULT IS IN AARGB0 MSB
								;AND AARGB1 LSB
	SWAPF	AARGB1, F			;DIVIDE THE RESULT BY 16 (COMBINATION OF A SCALING FACTOR AND A MULTIPLY BY TS)
	MOVLW	0FH					;
	ANDWF	AARGB1, F			;SET UPPER NIBBLE OF AARGB1 = TO LOWER NIBBLE
	BTFSC	AARGB0, 0			;OF AARGB0
	BSF		AARGB1, 4
	BTFSC	AARGB0, 1
	BSF		AARGB1, 5
	BTFSC	AARGB0, 2
	BSF		AARGB1, 6
	BTFSC	AARGB0, 3
	BSF		AARGB1, 7
	SWAPF	AARGB0, F			
	MOVLW	0FH					;SET UPPER NIBBLE TO 0 OR F TO KEEP PROPER SIGN
	ANDWF	AARGB0,	F
	MOVLW	0F0H
	BTFSC	AARGB0 ,3			;OLD SIGN BIT
	ADDWF	AARGB0,	F
	MOVF	AARGB1,	W			;
	ADDWF	INT_TERM_L,	F		;ADD THE LOWER BYTES TOGETHER
	BTFSC	STATUS, C			;ADD THE CARRY IF ANY
	INCF	INT_TERM_H,	F
	MOVF	AARGB0,	W			;ADD THE UPPER BYTES TOGETHER
	ADDWF	INT_TERM_H,	F		;
	GOTO	PID_DIF
SUME_NEG						;CURRENT INTEGRAL TERM IS NEGATIVE
	BTFSS	EN0,	7
	GOTO	ADD_INT_TERM		;SUME WILL GET CLOSER TO ZERO
	MOVLW	B'00010100'			;20 DECIMAL = B'00010100'
	ADDWF	INT_TERM_H,	W
	MOVWF	TEMP2
	BTFSS	TEMP2,	7			;IF BIT 7 IS SET THE RESULT IS NEGATIVE  
	GOTO	ADD_INT_TERM		;OK TO ADD THE TERM SUME IS NOT 2 BIG
	GOTO	PID_DIF				;
CHECK_2_BIG
	MOVLW	B'11101100'			;-20 DECIMAL = B'11101100'
	ADDWF	INT_TERM_H,	W
	MOVWF	TEMP2
	BTFSC	TEMP2,	7			;IF BIT 7 IS CLEAR THE RESULT IS POSITIVE  
	GOTO	ADD_INT_TERM		;OK TO ADD THE TERM SUME IS NOT 2 BIG

PID_DIF							;CALCULATE THE DIFFERENTIAL TERM
								;DERIVATIVE TERM CALCULATED USING FOLLOWING EQUATION
								;KD(EN0-EN3)/(KP*X*3*TS)
	MOVF	EN0,	W			;WHERE X IS AN UNKNOW SCALING FACTOR
	MOVWF	AARGB0			
	MOVF	EN3,	W
	SUBWF	AARGB0, F
	MOVF	KD,		W
	MOVWF	BARGB0
	CALL	FXM0808S			;CALL THE MULTIPLY SUBROUTINE 16 BIT RESULT IS IN AARGB0 MSB
								;AND AARGB1 LSB
	SWAPF	AARGB0, F			;DIVIDE BY 16 (PRECALCULATED KP*X*3*TS)
	MOVLW	0F0H				;MASK OFF LOWER NIBBLE
	ANDWF	AARGB0, F			
	SWAPF	AARGB1, F			;GET 4 MSB OF AARGB1 AND ADD THEM TO AARGB0
	MOVLW	0FH					;MACK OFF THE UPPER NIBBLE
	ANDWF	AARGB1, W
	ADDWF	AARGB0, W			;STORE IN WORKING 
	MOVWF	DER_TERM_H			;SET THE UPPER BYTE OF THE RESULT, IGNORE LOWER BYTE 
	BTFSC	DER_TERM_H, 7
	GOTO	TEST_SMALL			;SEE IF THE RESULT IS TOO SMALL
TEST_LARGE	
	MOVLW	B'11101100'			;-20 DECIMAL 
	ADDWF	DER_TERM_H,	W
	BTFSS	STATUS,	C			;IF CARRY IS SET DER_TERM_H IS TOO BIG  
	GOTO	PID_PROP			;THE TERM IS NOT 2 BIG
	MOVLW	B'00010100'			;20 DECIMAL
	MOVWF	DER_TERM_H
	GOTO 	PID_PROP
TEST_SMALL	
	MOVLW	B'00010100'			;20 DECIMAL
	ADDWF	DER_TERM_H,	W
	BTFSC	STATUS,	C			;IF CARRY IS NOT SET DER_TERM_H IS TOO SMALL  
	GOTO	PID_PROP			;THE TERM IS NOT 2 BIG
	MOVLW	B'11101100'			;-20 DECIMAL 
	MOVWF	DER_TERM_H
PID_PROP						;SUM THE TERMS
	MOVF	EN0,	W			;UP TO +/-85
	MOVWF	AARGB0
	MOVF	INT_TERM_H,	W		;UP TO +/-20
	ADDWF	AARGB0,	F
	MOVF	DER_TERM_H, W		;UP TO +/-20
	ADDWF	AARGB0,	F
	MOVF	KP,		W			;MULTIPLY THE SUMMED TERMS BY KP
	MOVWF	BARGB0
	CALL	FXM0808S			;CALL THE MULTIPLY SUBROUTINE 16 BIT RESULT IS IN AARGB0 MSB
								;AND AARGB1 LSB
SET_MOTOR
	BTFSS	AARGB0, 7			;TEST IF ERROR IS NEGATIVE
	GOTO	SET_FWD				;FULL TERM IS NEGATIVE CHANGE MOTOR TO REVERSE
SET_REV
	BSF		CCP1CON, 7
	BSF		CCP1CON, 6
	COMF	AARGB0,	F			;CONVERT TO A POSITIVE NUMBER
	COMF	AARGB1, F			
	INCF	AARGB1,	F
	BTFSC	STATUS, C
	INCF	AARGB0,	F			;
	GOTO 	SET_PWM
SET_FWD
	BCF		CCP1CON, 7
	BSF		CCP1CON, 6
SET_PWM
	MOVLW	B'11110000'			;TEST TO SEE IF RESULT TO TOO LARGE
	ANDWF	AARGB0, W
	BTFSC	STATUS,	Z			;IF ZERO IS SET ITS OK TO MULTIPLY BY 8
	GOTO	SCALE
	MOVLW	3FH					;WHEN MULTIPLIED BY 4 VALUE IS TOO BIG
	MOVWF	CCPR1L				;SET TO MAX SPEED
	GOTO	SHIFT_ERRORS
SCALE							;MULTIPLY BY 4 ROUTINE TO SCALE RESULT TO PROPER MOTOR SPEED
	RLF		AARGB0,	F
	RLF		AARGB0,	F
	BCF		AARGB0, 1
	BCF		AARGB0, 0
	BTFSC	AARGB1, 7
	BSF		AARGB0, 1	
	BTFSC	AARGB1, 6
	BSF		AARGB0, 0	
	MOVF	AARGB0,	W
	MOVWF	CCPR1L				;SET 10 BIT PWM 8MSB
	BCF		CCP1CON,5			;SET 10 BIT PWM 2LSB
	BCF		CCP1CON,4
	BTFSC	AARGB1, 5
	BSF		CCP1CON,5
	BTFSC	AARGB1, 4
	BSF		CCP1CON,4
SHIFT_ERRORS					;CHANGE THE ERROR TERMS
	MOVF	EN2,	W	
	MOVWF	EN3	
	MOVF	EN1,	W
	MOVWF	EN2	
	MOVF	EN0,	W
	MOVWF	EN1	
	CLRF	DO_PID				;CLEAR PID LOOP FLAG
	BCF		PORTA, 4
	GOTO	MAIN

DELAY_SUB
	DECFSZ	TEMP2,	F
	GOTO 	DELAY_SUB
	RETURN
	
		
;8x8 bit signed fixed point multiply 8x8 -> 16
;Input:		8 bit signed fixed point multiplicand in AARGB0
;			8 bit signed fixed point multiplier in BARGB0
;Result:	AARG
;Max Timing	12+69+2=83 clks	B>0
;			17+69+2=88 clks B<0
;Min Timing	12+46=59 clks
;			6 clks			A=0
FXM0808S	
	CLRF	AARGB1				;CLEAR PARTIAL PRODUCT
	CLRF	SIGN
	MOVF	AARGB0,	W
	BTFSC	STATUS, Z
	RETLW	00H
	
	XORWF	BARGB0,	W
	MOVWF	TEMPB3
	BTFSC	TEMPB3, 7
	COMF	SIGN, F

	BTFSS	BARGB0, 7
	GOTO	M0808SOK
	COMF	BARGB0,	F			;MAKE MULTIPLIER BARG > 0
	INCF	BARGB0,F
	COMF	AARGB0,F
	INCF	AARGB0,F

	BTFSC	BARGB0, 7
	GOTO	M0808SX
M0808SOK

SMUL0808L
	MOVLW	07H
	MOVWF	LOOPCOUNT
	MOVF	AARGB0, W
LOOPSM0808A
	RRF		BARGB0,	F
	BTFSC	STATUS,	C	
	GOTO	LSM0808NA
	DECFSZ	LOOPCOUNT,	F
	GOTO	LOOPSM0808A

	CLRF	AARGB0
	RETLW	00H
LOOPSM0808
	RRF		BARGB0,	F
	BTFSC	STATUS,	C
	ADDWF	AARGB0,	F
LSM0808NA
	RLF		SIGN, 	F	
	RRF		AARGB0,	F
	RRF		AARGB1,	F
	DECFSZ	LOOPCOUNT,	F
	GOTO	LOOPSM0808
	RLF		SIGN,	F
	RRF		AARGB0,	F
	RRF		AARGB1,	F
	RETLW	00H
M0808SX
	CLRF	AARGB1
	RLF		SIGN, W
	RRF		AARGB0,F
	RRF		AARGB1,F
	RETLW	00H


	END