;**********************************************************************
;*                                                                    *
;用此軟件控制高壓直流無刷電機的運轉(zhuǎn)(2007/6/10)
;電機轉(zhuǎn)速低于2500RPM時閉環(huán)控制
;*                                                                    *
;**********************************************************************
;    Filename:	    SensBLDC.asm                                      *
;**********************************************************************
;    Files required: p16f877.inc                                      *
;**********************************************************************
;    Notes: Sensored brushless motor control                          *
;**********************************************************************

	list      p=16f877a            ; list directive to define processor
	#include <p16f877a.inc>        ; processor specific variable definitions
	

;**********************************************************************
;*
;* Define variable storage
;*
	CBLOCK 0x20

	ADC		; PWM threshold is ADC result
	adctemp	;當前ADC的值
	adctemp1
	LastSensor	; last read motor sensor data
	DriveWord	; six bit motor drive data
	sensortemp
	flag
	delaycount1
	delaycount2
	timer2ifcount
	timer1ifcount	;用于增加TIMER1的時間
	timer1ifcounttemp
	timer2ifcounttemp
	pulse			;當前轉(zhuǎn)速脈沖數(shù)
	prepulse
	pulsetemp1
	pulsetemp2
	pulsetemp3
	pulse0			;規(guī)定的轉(zhuǎn)速脈沖數(shù)
	e1
	e1temp				;當前轉(zhuǎn)速偏差
	e0				;上一次轉(zhuǎn)速偏差
	e0temp
	z1				;當前規(guī)正轉(zhuǎn)速偏差
	z1temp
	z0				;上一次
	z0temp
	de1				;轉(zhuǎn)速偏差的偏差
	de1temp
	de0
	de0temp
	deltw1			;查表偏移值
	deltw2			;delte查表偏移值
	fuzzyflag		;模糊計算標志
	wtemp
	ENDC

;**********************************************************************
;*
;* Define I/O
;*

#define	OffMask		B'00000000'
#define	DrivePort	PORTB
#define DrivePortTris	TRISB
#define	SensorMask	B'00000111'	;RA2,RA3,RA4分別對應(yīng)SA,SB,SC
#define	SensorPort	PORTA

#define led1 portd,0	;燈1
#define led2 portd,1	;燈2
#define led3 portd,2	;燈3
#define RUNSTOPKEY PORTD,6	;運行停止鍵
#define directionkey portd,7 ;改變方向鍵
#define directionbit flag,0	;方向標志位
#define runstopbit flag,1	;運行標志,1代表運行,0代表停止
#define starttime flag,2	;軟啟動開始
#define errorflag flag,3	;錯誤標志
#define voltageerrorflag flag,4
#define led4 portc,0	;燈4
#define overcurrent portc,1	;過流檢測
#define voltagefault portc,2 ;電壓檢測

#define overtemp porte,2	;IGBT模塊溫度檢測

#define startadc d'0'	;電機啟動時的ADC
#define startendadc d'50'	;電機啟動結(jié)束時的ADC
#define deltadc d'8'	;啟動時增加的ADC
#define refmin d'25'
#define adcmin d'20'
#define deltstopadc d'8'	;停止時減小的ADC

模糊控制參數(shù)
#define tmr1hstart d'15'
#define tmr1lstart d'0' 
#define timer1ifcountmax d'5'	;采樣時間為500ms
#define prescale d'3'
#define edn6 d'0'	;e的模糊數(shù)等級
#define edn5 d'1'
#define edn4 d'2'
#define edn3 d'3'
#define edn2 d'4'
#define edn1 d'5'
#define ed0 d'6'
#define edp1 d'7'
#define edp2 d'8'
#define edp3 d'9'
#define edp4 d'10'
#define edp5 d'11'
#define edp6 d'12'

#define dedn6 d'1'	;de的模糊數(shù)等級
#define dedn5 d'14'
#define dedn4 d'27'
#define dedn3 d'40'
#define dedn2 d'53'
#define dedn1 d'66'	
#define ded0 d'79'
#define dedp1 d'92'
#define dedp2 d'105'
#define dedp3 d'118'
#define dedp4 d'131'
#define dedp5 d'144'
#define dedp6 d'157'

#define fodn6 d'168'	;模糊輸出等級adc=40
#define fodn55 d'140'
#define fodn5 fodn55-d'2'
#define fodn45 fodn55-d'3'
#define fodn4 fodn55-d'4'
#define fodn35 fodn55-d'5'
#define fodn3 fodn55-d'6'
#define fodn25 fodn55-d'7'
#define fodn2 fodn55-d'8'
#define fodn15 fodn55-d'9'
#define fodn1 fodn55-d'10'
#define fodn05 fodn55-d'11'
#define fod0 d'0'
#define fodp6 d'40'	;模糊輸出等級adc=200
#define fodp55 d'12'
#define fodp5 fodp55-d'2'
#define fodp45 fodp55-d'3'
#define fodp4 fodp55-d'4'
#define fodp35 fodp55-d'5'
#define fodp3 fodp55-d'6'
#define fodp25 fodp55-d'7'
#define fodp2 fodp55-d'8'
#define fodp15 fodp55-d'9'
#define fodp1 fodp55-d'10'
#define fodp05 fodp55-d'11'
#define fon6adc d'40'
#define fop6adc d'200'
#define speedoverflag fuzzyflag,0
#define firstflag fuzzyflag,1 

;**********************************************************************
	org	0x000		; startup vector
	nop			; required for ICD operation
	clrf    PCLATH          ; ensure page bits are cleared
  	goto    Initialize      ; go to beginning of program


	ORG     0x004           ; interrupt vector location
;	call closedeltadcsub
	retfie                  ; return from interrupt

;**********************************************************************
;*
;* Initialize I/O ports and peripherals
;*

Initialize
	clrf	DrivePort	; all drivers off

	banksel TRISA
; setup I/O
	MOVLW B'11000000'	;PORTD7,6輸入,其他輸出
	movwf trisd
	MOVLW B'11111110'	;PORTC0輸出,其他輸入
	MOVWF TRISC

	clrf	DrivePortTris	; set motor drivers as outputs
	movlw	B'00011111'	; A/D on RA1,  Motor sensors on RA<4:2>
	movwf	TRISA		; 
; setup Timer0
	movlw	B'11010010'	; Timer0: Fosc, 1:4
	movwf	OPTION_REG
; Setup ADC (bank1)
	movlw	B'00001110'	; ADC left justified, AN0 ONLY
	movwf	ADCON1

	banksel	ADCON0
; setup ADC (bank0)
	movlw	B'11000001'	; ADC clock from int RC, AN0, ADC on
	movwf	ADCON0

	bsf	ADCON0,GO	; start ADC
	movlw b'01111111'	;timer2的prescale為1:16,后分頻比為1:16,on,
	movwf t2con			;用于判斷電機是否堵轉(zhuǎn)
	clrf portd
	clrf flag
	clrf LastSensor	; initialize last sensor reading
	call Commutate	; determine present motor positon
	clrf ADC		; start speed control threshold at zero until first ADC reading
	bsf led1	;燈1亮
	bcf led2
	bcf led3
	bcf led4
mainloop
	btfss runstopkey	;按下runstopkey?
	goto mainrunloop
	btfss directionkey	;按下DIRECTIONKEY
	call directionsub	;是的則DIRECTIONSUB
	goto mainloop
mainrunloop		;運行
	clrf flag
	bcf led2
	bcf led3
	bcf led4
	call readadcsub
	bsf led2
	call commutate
	clrf tmr2
	clrf timer2ifcount
	clrf timer1ifcount
	clrf pulse
	clrf fuzzyflag
	clrf tmr1L	;TIMER1為0
	clrf tmr1h
	clrf t1con
	bsf t1con,tmr1on	;啟動TIMER1
	bsf starttime	;電機處于起動時間
	movlw STARTADC	;開始啟動時ADC的值
	movwf adc
increaserun	;加速運行(軟啟動)
	btfss starttime	;起動結(jié)束?
	goto normalrun	;正常運轉(zhuǎn)
	BTFSs PIR1,TMR1IF	;TIMER1時間到
	goto normalrun	;正常運轉(zhuǎn)
	bcf pir1,tmr1if	;清除tmr1if
	movlw deltadc
	addwf adc,f
	btfsc status,c	;是否超過255?
	goto adcgeadctemp	;是的則ADC>=ADCTEMP
	movlw startendadc		;沒有超過則比較startendadc
	subwf adc,w
	btfss status,c
	goto normalrun	;adc<startendadc
adcgeadctemp		;adc>=startendadc
;	movf adctemp,w
	movlw d'100'
	movwf adc
;	movlw d'130'
;	movwf pulse0
	bcf starttime

	bcf t1con,tmr1on	;關(guān)閉TIMER1的運行
;	banksel pie1
;	bsf pie1,0	;開放TMR1中斷使能
;	banksel t1con
;	movlw 0c0h
;	movwf intcon
	movlw b'00110000'
	movwf t1con
	movlw tmr1lstart
	movwf TMR1L
	movlw tmr1hstart
	movwf tmr1h
	clrf e1
	clrf e0
	clrf z1
	clrf z0
	clrf de1
	clrf de0
	clrf pulse
	clrf prepulse
	clrf timer1ifcount
	clrf pulsetemp1
	clrf pulsetemp2
	bsf t1con,tmr1on
	bsf firstflag
normalrun
	btfss starttime
	call readadcsub	;運行時讀取ADCTEMP
	call runsub
	movf adc,w
	movwf adctemp1
	btfss starttime
	call closedeltadcsub	;閉環(huán)控制
	btfss voltagefault	;如電壓超過242V或低于170V則電機停止運轉(zhuǎn)
	goto voltagefaultstoprun
	btfsc errorflag
	goto errorstoprun
	btfsc directionkey	;是否停止?
	goto increaserun
	goto stoprun

errorstoprun
	bsf led4
	goto stoprun
voltagefaultstoprun
	bsf led4
	bsf led3
	bsf voltageerrorflag
stoprun
	call norunsub	;減速停止
	bcf led2
	call delaysub
	call delaysub
	goto mainloop


;閉環(huán)時DELTADC計算子程序
closedeltadcsub
	btfss pir1,tmr1if	;TIMER1溢出?
	return	;沒有溢出則返回
	bcf pir1,tmr1if
	bcf t1con,tmr1on
	movlw tmr1lstart
	movwf tmr1l
	movlw tmr1hstart
	movwf tmr1h
	bsf t1con,tmr1on
	incf timer1ifcount,f	;timer1count=38?
	movlw timer1ifcountmax
	subwf timer1ifcount,w
	btfss status,z
	return
	btfss led2
	goto setled2
	bcf led2
	goto led2start
setled2
	bsf led2
led2start
	btfss firstflag
	goto secondtime
	movf pulse,w
	movwf pulsetemp1
	bcf firstflag
	goto secondtimeend
secondtime
	bcf status,c
	rrf pulsetemp1,f
	bcf status,c
	rrf pulse,f
	movf pulsetemp1,w
	addwf pulse,f
	movf pulse,w
	movwf pulsetemp1
secondtimeend
	movf timer1ifcount,w
	movwf timer1ifcounttemp
	movlw d'255'	;如PULSE=255則不進行閉環(huán)控制
	subwf pulse,w
	btfsc status,z
	return

	movf pulse0,w				;該次速度脈沖數(shù)計算結(jié)束,需要計算DELTADC
	SUBWF PULSE,w
	movwf e1				;e1=pulse-pulse0
	btfss status,c			;e1>=0
	goto e1ltzero			;e1<0則轉(zhuǎn)到E1LTZERO
	btfsc status,z
	goto e1eqzero			;e1=0
	movlw d'2'				;e1=1?
	subwf e1,w
	btfss status,c
	goto e1eqp1				;e1=1
	movlw d'4'				;e1=2?
	subwf e1,w			
	btfss status,c
	goto e1eqp2				;e1=2
	movlw d'6'				;e1=3?
	subwf e1,w
	btfss status,c
	goto e1eqp3				;e1=3
	movlw d'8'				;e1=4?
	subwf e1,w
	btfss status,c
	goto e1eqp4				;e1=4
	movlw d'10'				;e1=5?
	subwf e1,w			
	btfss status,c
	goto e1eqp5				;e1=5
	movlw edp6				;轉(zhuǎn)速差大于30以上
	movwf deltw1						;e1>3(n>30rpm)
	goto e1pend
e1eqzero
	movlw ed0			;轉(zhuǎn)速差為ZERO
	movwf deltw1
	goto e1pend
e1eqp1
	movlw edp1			;轉(zhuǎn)速差為10RPM
	movwf deltw1
	goto e1pend
e1eqp2
	movlw edp2			;轉(zhuǎn)速差為20RPM
	movwf deltw1
	goto e1pend
e1eqp3	
	movlw edp3			;轉(zhuǎn)速差為30RPM
	movwf deltw1
	goto e1pend
e1eqp4
	movlw edp4
	movwf deltw1
	goto e1pend
e1eqp5
	movlw edp5
	movwf deltw1
e1pend
	goto e1end

e1ltzero
	movf pulse,w
	subwf pulse0,w
	movwf e1
	movlw d'2'				;e1=-1?
	subwf e1,w
	btfss status,c
	goto e1eqn1				;e1=-1
	movlw d'4'				;e1=-2?
	subwf e1,w			
	btfss status,c
	goto e1eqn2				;e1=-2
	movlw d'6'				;e1=-3?
	subwf e1,w
	btfss status,c
	goto e1eqn3				;e1=-3
	movlw d'8'				;e1=-4?
	subwf e1,w			
	btfss status,c
	goto e1eqn4				;e1=-4
	movlw d'10'				;e1=-5?
	subwf e1,w
	btfss status,c
	goto e1eqn5				;e1=-3
	movlw edn6				;轉(zhuǎn)速差大于-30以上
	movwf deltw1						;e1>-3(n>30rpm)
	goto e1nend
e1eqn1
	movlw edn1			
	movwf deltw1
	goto e1nend
e1eqn2
	movlw edn2			
	movwf deltw1
	goto e1nend
e1eqn3	
	movlw edn3			
	movwf deltw1
	goto e1nend
e1eqn4
	movlw edn4			
	movwf deltw1
	goto e1nend
e1eqn5	
	movlw edn5			
	movwf deltw1
e1nend
e1end

	movf deltw1,w		;計算轉(zhuǎn)速偏差的偏差
	movwf z1	
	movf z0,w			
	SUBWF z1,w
	movwf de1				;de1=z1-z0
	btfss status,c			;de1>=0
	goto de1ltzero			;de1<0則轉(zhuǎn)到dE1LTZERO
	btfsc status,z
	goto de1eqzero			;de1=dez
	movlw d'1'				;de1=1?
	subwf de1,w
	btfsc status,z
	goto de1eqp1			;de1=1
	movlw d'2'				;de1=2?
	subwf de1,w
	btfsc status,z
	goto de1eqp2			;de1=2
	movlw d'3'				;de1=3?
	subwf de1,w
	btfsc status,z
	goto de1eqp3			;de1=3	
	movlw d'4'				;de1=4?
	subwf de1,w
	btfsc status,z
	goto de1eqp4			;de1=4
	movlw d'5'				;de1=5?
	subwf de1,w
	btfsc status,z
	goto de1eqp5			;de1=5
	movlw dedp6				;de1=dep6
	movwf deltw2			
	goto de1pend
de1eqzero
	movlw ded0			;de1=dez
	movwf deltw2
	goto de1pend
de1eqp1
	movlw dedp1			;de1=1
	movwf deltw2
	goto de1pend
de1eqp2
	movlw dedp2			;de1=2
	movwf deltw2
	goto de1pend
de1eqp3
	movlw dedp3			;de1=3
	movwf deltw2
	goto de1pend
de1eqp4
	movlw dedp4			;de1=4
	movwf deltw2
	goto de1pend
de1eqp5
	movlw dedp5			;de1=5
	movwf deltw2
de1pend
	goto de1end

de1ltzero
	movf z1,w
	subwf z0,w
	movwf de1
	movlw d'1'				;de1=-1?
	subwf de1,w
	btfsc status,z
	goto de1eqn1				;de1=-1
	movlw d'2'				;de1=-2?
	subwf de1,w
	btfsc status,z
	goto de1eqn2				;de1=-2
	movlw d'3'				;de1=-3?
	subwf de1,w
	btfsc status,z
	goto de1eqn3				;de1=-3
	movlw d'4'				;de1=-4?
	subwf de1,w
	btfsc status,z
	goto de1eqn4				;de1=-4