movlw d'5'				;de1=-5?
	subwf de1,w
	btfsc status,z
	goto de1eqn5				;de1=-5

	movlw dedn6				;de1=-6
	movwf deltw2		
	goto de1nend
de1eqn1
	movlw dedn1			;de1=-1
	movwf deltw2
	goto de1nend
de1eqn2
	movlw dedn2			;de1=-2
	movwf deltw2
	goto de1nend
de1eqn3
	movlw dedn3			;de1=-3
	movwf deltw2
	goto de1nend
de1eqn4
	movlw dedn4			;de1=-4
	movwf deltw2
	goto de1nend
de1eqn5
	movlw dedn5			;de1=-5
	movwf deltw2

de1nend
de1end
	movf e0,w
	movwf e0temp
	movf e1,w
	movwf e1temp
	movwf e0
	movf de0,w
	movwf de0temp
	movf de1,w
	movwf de1temp
	movwf de0
	movf z0,w
	movwf z0temp
	movf z1,w
	movwf z0
	movwf z1temp
	clrf pulse
	clrf prepulse
	clrf timer1ifcount	;重新開始計數(shù)
	clrf tmr1l
	clrf tmr1h
;	movlw d'79'
;	movwf deltw2	
;	return
;根據DELTW1和DELTW2查表求DELTADC
	movlw	HIGH DELTADCTable	; get MS byte of table
	movwf	PCLATH		; prepare for computed GOTO
	movlw	LOW deltadcTable	; get LS byte of table
	addwf	DELTW1,w
	btfsc	STATUS,C	; page change in table?
	incf	PCLATH,f	; yes - adjust MS byte
	ADDWF DELTW2,W
	BTFSC STATUS,C
	INCF PCLATH,F
	CALL DELTADCTABLE
	movwf wtemp
	btfss wtemp,7	;DELTADC是否為負值
	goto adddeltadc	;為正則adc+wtemp
	call checkwtempsub
	bcf wtemp,7	;為負值則adc-wtemp
;	decfsz wtemp,w	;WTEMP=129?
;	goto wtempneq129
;	movlw fon6adc
;	movwf adc
;	return
;wtempneq129
	movf wtemp,w
	subwf adc,f
	btfsc status,c
	goto adcltadcmin
	movlw adcmin	;adc<wtemp則ADC=0
	movwf adc
	RETURN
adcltadcmin
	movlw adcmin
	subwf adc,w
	btfsc status,c
	return
	movlw adcmin
	movwf adc
adddeltadc
	call checkwtempsub
;	decfsz wtemp,w	;wtemp=1
;	goto wtempneq1
;	movlw fop6adc
;	movwf adc
;	return
;wtempneq1
	movf wtemp,w
	addwf adc,f
	btfss status,c
	return
	movlw d'255'	;超過255時ADC=255
	movwf adc
	return	

;檢查WTEMP值是否過大
checkwtempsub
	movlw d'20'		;pulse<25?
	subwf pulsetemp1,w
	btfsc status,c	
	goto pulsege25	;pulse>=25
	movlw d'5'		;pulse<25
	subwf wtemp,w
	btfss status,c
	return
	movlw d'5'
	movwf wtemp
	return
pulsege25
	movlw d'25'		;25<pulse<30?
	subwf pulsetemp1,w
	btfsc status,c	
	goto pulsege30	;pulse>=30
	movlw d'8'		;pulse<30
	subwf wtemp,w
	btfss status,c
	return
	movlw d'8'
	movwf wtemp
	return
pulsege30
	movlw d'30'		;pulse<35?
	subwf pulsetemp1,w
	btfsc status,c	
	return			;pulse>=35
	movlw d'10'		;pulse<35
	subwf wtemp,w
	btfss status,c
	return
	movlw d'10'
	movwf wtemp
	return

DELTADCTABLE
	MOVWF PCL
	RETLW fodp6		;EN6-DEN6
	RETLW FODP55		;EN5-DEN6	
	RETLW FODP5		;EN4-DEN6
	RETLW FODP45		;EN3-DEN6
	RETLW FODP4		;EN2-DEN6
	RETLW FODP35		;EN1-DEN6
	RETLW FODP3		;E0-DEN6
	RETLW FODP25		;EP1-DEN6
	RETLW FODp2		;EP2-DEN6
	RETLW FODp15		;EP3-DEN6
	RETLW FODp1		;EP4-DEN6
	RETLW FODp05		;EP5-DEN6
	RETLW FOD0		;EP6-DEN6
		
	RETLW FODP55		;EN6-DEN5
	RETLW FODP5		;EN5-DEN5	
	RETLW FODP45		;EN4-DEN5
	RETLW FODP4		;EN3-DEN5
	RETLW FODP35		;EN2-DEN5
	RETLW FODP3		;EN1-DEN5
	RETLW FODP25		;E0-DEN5
	RETLW FODP2		;EP1-DEN5
	RETLW FODp15		;EP2-DEN5
	RETLW FODp1		;EP3-DEN5
	RETLW FODp05		;EP4-DEN5
	RETLW FOD0		;EP5-DEN5
	RETLW FODn05		;EP6-DEN5
	
	RETLW FODP5		;EN6-DEN4
	RETLW FODP45		;EN5-DEN4	
	RETLW FODP4		;EN4-DEN4
	RETLW FODP35		;EN3-DEN4
	RETLW FODP3		;EN2-DEN4
	RETLW FODP25		;EN1-DEN4
	RETLW FODP2		;E0-DEN4
	RETLW FODp15		;EP1-DEN4
	RETLW FODp1		;EP2-DEN4
	RETLW FODp05		;EP3-DEN4
	RETLW FOD0		;EP4-DEN4
	RETLW FODN05		;EP5-DEN4
	RETLW FODN1		;EP6-DEN4
	
	RETLW FODP45		;EN6-DEN3
	RETLW FODP4		;EN5-DEN3	
	RETLW FODP35		;EN4-DEN3
	RETLW FODP3		;EN3-DEN3
	RETLW FODP25		;EN2-DEN3
	RETLW FODP2		;EN1-DEN3
	RETLW FODP15		;E0-DEN3
	RETLW FODP1		;EP1-DEN3
	RETLW FODp05		;EP2-DEN3
	RETLW FOD0		;EP3-DEN3
	RETLW FODN05		;EP4-DEN3
	RETLW FODN1		;EP5-DEN3
	RETLW FODN15		;EP6-DEN3
	
	RETLW FODP4		;EN6-DEN2
	RETLW FODP35		;EN5-DEN2	
	RETLW FODP3		;EN4-DEN2
	RETLW FODP25		;EN3-DEN2
	RETLW FODP2		;EN2-DEN2
	RETLW FODP15		;EN1-DEN2
	RETLW FODP1		;E0-DEN2
	RETLW FODp05		;EP1-DEN2
	RETLW FOD0		;EP2-DEN2
	RETLW FODN05		;EP3-DEN2
	RETLW FODN1	;EP4-DEN2
	RETLW FODN15		;EP5-DEN2
	RETLW FODN2		;EP6-DEN2
	
	RETLW FODP35		;EN6-DEN1
	RETLW FODP3		;EN5-DEN1	
	RETLW FODP25		;EN4-DEN1
	RETLW FODP2		;EN3-DEN1
	RETLW FODP15		;EN2-DEN1
	RETLW FODP1		;EN1-DEN1
	RETLW FODP05		;E0-DEN1
	RETLW FOD0		;EP1-DEN1
	RETLW FODN05		;EP2-DEN1
	RETLW FODN15		;EP3-DEN1
	RETLW FODN2		;EP4-DEN1
	RETLW FODN25		;EP5-DEN1
	RETLW FODN3		;EP6-DEN1
	
	RETLW FODP3		;EN6-DE0
	RETLW FODP25		;EN5-DE0	
	RETLW FODP2		;EN4-DE0
	RETLW FODP15		;EN3-DE0
	RETLW FODP1		;EN2-DE0
	RETLW FODP05		;EN1-DE0
	RETLW FOD0		;E0-DE0
	RETLW FODN05	;EP1-DE0
	RETLW FODN1		;EP2-DE0
	RETLW FODN15		;EP3-DE0
	RETLW FODN2		;EP4-DE0
	RETLW FODN25		;EP5-DE0
	RETLW FODN3		;EP6-DE0
	
	RETLW FODP25		;EN6-DEP1
	RETLW FODP2		;EN5-DEP1	
	RETLW FODP15		;EN4-DEP1
	RETLW FODP1		;EN3-DEP1
	RETLW FODP05		;EN2-DEP1
	RETLW FOD0		;EN1-DEP1
	RETLW FODn05		;E0-DEP1
	RETLW FODn1		;EP1-DEP1
	RETLW FODn15		;EP2-DEP1
	RETLW FODN2		;EP3-DEP1
	RETLW FODN25		;EP4-DEP1
	RETLW FODN3		;EP5-DEP1
	RETLW FODN35		;EP6-DEP1
	
	RETLW FODP2		;EN6-DEP2
	RETLW FODP15		;EN5-DEP2	
	RETLW FODP1		;EN4-DEP2
	RETLW FODP05		;EN3-DEP2
	RETLW FOD0		;EN2-DEP2
	RETLW FODn05		;EN1-DEP2
	RETLW FODn1		;E0-DEP2
	RETLW FODn15		;EP1-DEP2
	RETLW FODn2		;EP2-DEP2
	RETLW FODN25		;EP3-DEP2
	RETLW FODN3		;EP4-DEP2
	RETLW FODN35		;EP5-DEP2
	RETLW FODN4		;EP6-DEP2
	
	RETLW FODP15		;EN6-DEP3
	RETLW FODP1		;EN5-DEP3	
	RETLW FODP05		;EN4-DEP3
	RETLW FOD0		;EN3-DEP3
	RETLW FODn05		;EN2-DEP3
	RETLW FODn1		;EN1-DEP3
	RETLW FODn15		;E0-DEP3
	RETLW FODn2		;EP1-DEP3
	RETLW FODn25		;EP2-DEP3
	RETLW FODN3		;EP3-DEP3
	RETLW FODN35		;EP4-DEP3
	RETLW FODN4		;EP5-DEP3
	RETLW FODN45		;EP6-DEP3
	
	RETLW FODP1		;EN6-DEP4
	RETLW FODP05		;EN5-DEP4	
	RETLW FOD0		;EN4-DEP4
	RETLW FODn05		;EN3-DEP4
	RETLW FODn1		;EN2-DEP4
	RETLW FODn15		;EN1-DEP4
	RETLW FODn2		;E0-DEP4
	RETLW FODn25		;EP1-DEP4
	RETLW FODn3		;EP2-DEP4
	RETLW FODn35		;EP3-DEP4
	RETLW FODN4		;EP4-DEP4
	RETLW FODN45		;EP5-DEP4
	RETLW FODN5		;EP6-DEP4
	
	RETLW FODP05		;EN6-DEP5
	RETLW FOD0	;EN5-DEP5	
	RETLW FODn05		;EN4-DEP5
	RETLW FODn1		;EN3-DEP5
	RETLW FODn15		;EN2-DEP5
	RETLW FODn2		;EN1-DEP5
	RETLW FODn25		;E0-DEP5
	RETLW FODn3		;EP1-DEP5
	RETLW FODn35		;EP2-DEP5
	RETLW FODN4	;EP3-DEP5
	RETLW FODN45		;EP4-DEP5
	RETLW FODN5		;EP5-DEP5
	RETLW FODN55		;EP6-DEP5
	
	RETLW FOD0		;EN6-DEP6
	RETLW FODn05		;EN5-DEP6	
	RETLW FODn1		;EN4-DEP6
	RETLW FODn15		;EN3-DEP6
	RETLW FODn2		;EN2-DEP6
	RETLW FODn25		;EN1-DEP6
	RETLW FODn3		;E0-DEP6
	RETLW FODn35		;EP1-DEP6
	RETLW FODn4		;EP2-DEP6
	RETLW FODn45		;EP3-DEP6
	RETLW FODN5		;EP4-DEP6
	RETLW FODN55		;EP5-DEP6
	RETLW FODN6		;EP6-DEP6
	


;
;運行停止標志改變子程序
RUNSTOPSUB
	CALL DELAYSUB	;
	btfsc runstopkey
	return
	movlw b'00000010'
	xorwf flag,f
	return
;方向標志改變子程序
directionsub
	call delaysub
	btfsc directionkey
	return
	movlw b'00000001'
	xorwf flag,f
	return

delaysub
	movlw d'50'
	movwf delaycount1
delayloop1
	movlw d'100'
	movwf delaycount2
delayloop2
	decfsz delaycount2
	goto delayloop2
	decfsz delaycount1
	goto delayloop1
	return

norunsub
;	banksel pie1
;	bcf pie1,0
;	banksel intcon 
;	clrf intcon
	clrf t1con
	clrf tmr1L	;TIMER1為0
	clrf tmr1h
	bsf t1con,tmr1on	;啟動TIMER1
decreaserun	;減速運行(軟停止)
	BTFSs PIR1,TMR1IF	;TIMER1時間到
	goto decreaserunbranch1	;正常運轉
	bcf pir1,tmr1if	;清除tmr1if
	movlw deltstopadc
	subwf adc,f
	btfss status,c	;是否小于0
	goto enddecreaserun
decreaserunbranch1
	call runsub
	goto decreaserun
enddecreaserun
	bcf t1con,tmr1on	;關閉TIMER1的運行
	clrf driveport
	bcf runstopbit
	return
;運行子程序
runsub
	call	Commutate	; test for commutation change
	incfsz	ADC,w		; if ADC is 0xFF we're at full speed - skip timer add
	goto	PWM		; add timer 0 to ADC for PWM 
	movf	DriveWord,w	; force on condition
	goto	Drive		; continue
PWM
	movf	ADC,w		; restore ADC reading
	addwf	TMR0,w		; add it to current timer0
	movf	DriveWord,w	; restore commutation drive data
	btfss	STATUS,C	; test if ADC + timer0 resulted in carry
	andlw	OffMask		; no carry - supress high,low drivers
;	nop
Drive
	btfsc overcurrent	;過流?
	goto drive2
	andlw offmask
	bsf led3
	goto drive1
drive2
	bcf led3
drive1	
	movwf	DrivePort	; enable motor drivers
;	return
lockcheck
	btfss pir1,tmr2if	;timer2溢出 
	return	;沒有則返回
	bcf pir1,tmr2if
	clrf tmr2
	incf timer2ifcount,f
	movlw d'100'
	subwf timer2ifcount,w
	btfss status,z	;timer2ifcount>某一固定值
	return	;不是則返回
	movf timer2ifcount,w
	movwf timer2ifcounttemp
	bsf errorflag	;是則電機發(fā)生錯誤
	clrf driveport	;電機停止運轉
	clrf timer2ifcount
	clrf tmr2
	bsf led4
	return
	
ReadADCsub
;**********************************************************************
;*
;* If the ADC is ready then read the speed control potentiometer
;* and start the next reading
;*
	btfsc	ADCON0,NOT_DONE	; is ADC ready?
	return			; no - return

	movf	ADRESH,w	; get ADC result
	bsf	ADCON0,GO	; restart ADC
	movwf	ADCtemp		; save result in speed control threshold
	movlw refmin
	subwf adctemp,w		;adc<adcmin?
	btfsc status,c	
	goto adctempgeadcmin
	movlw refmin	;adc<adcmin則ADC=ADCMIN
	movwf adctemp
	movwf pulse0
;	movwf adc
	return
adctempgeadcmin		;adc>=adcmin
	movf adctemp,w
	movwf pulse0
;	movwf adc
	return

;**********************************************************************
;*
;* Read the sensor inputs and if a change is sensed then get the
;* corresponding drive word from the drive table
;*
Commutate
	movf sensorport,w
	movwf sensortemp
	rrf sensortemp,f
	rrf sensortemp,f
	movlw	SensorMask	; retain only the sensor bits
	andwf	Sensortemp,f	; get sensor data
	movf sensortemp,w
	xorwf	LastSensor,w	; test if motion sensed
	btfsc	STATUS,Z	; zero if no change
	return			; no change - back to the PWM loop
	movf sensortemp,w
	movwf lastsensor
	clrf tmr2
	clrf timer2ifcount	
;	xorwf	LastSensor,f	; replace last sensor data with current
;	incf pulse,f
	incf prepulse,f
	movlw prescale
	subwf prepulse,w
	btfss status,z
	goto pulsegt256
	clrf prepulse
	incfsz pulse,f
	goto pulsegt256
	movlw d'255'
	movwf pulse
	bsf speedoverflag
pulsegt256
	btfss	DirectionBit	; test direction bit
	goto	FwdCom		; bit is zero - do forward commutation

				; reverse commutation
	movlw	HIGH RevTable	; get MS byte of table
	movwf	PCLATH		; prepare for computed GOTO
	movlw	LOW RevTable	; get LS byte of table
	goto	Com2
FwdCom				; forward commutation
	movlw	HIGH FwdTable	; get MS byte of table
	movwf	PCLATH		; prepare for computed GOTO
	movlw	LOW FwdTable	; get LS byte of table
Com2
	addwf	LastSensor,w	; add sensor offset
	btfsc	STATUS,C	; page change in table?
	incf	PCLATH,f	; yes - adjust MS byte

	call	GetDrive	; get drive word from table
	movwf	DriveWord	; save as current drive word

	return

GetDrive
	movwf	PCL
;**********************************************************************
;*
;* The drive tables are built based on the following assumptions:
;* 1) There are six drivers in three pairs of two
;* 2) Each driver pair consists of a high side (+V to motor) and low side (motor to ground) drive
;*3)ra2=sa,ra3=sb,ra4=sc
;*4)portb(5.4.3.2.1.0)控制L3.H3.H2.L2.H1.L1
FwdTable
	retlw	B'00000000'	; invalid(000)
	retlw	B'00100010'	; scsbsa(001)
	retlw	B'00001001'	; scsbsa(010)
	retlw	B'00101000'	; scsbsa(011)
	retlw	B'00010100'	; scsbsa(100)
	retlw	B'00000110'	; scsbsa(101)
	retlw	B'00010001'	; scsbsa(110)
	retlw	B'00000000'	; invalid(111)
RevTable
	retlw	B'00000000'	; invalid(000)
	retlw	B'00010001'	; scsbsa(001)/
	retlw	B'00000110'	; scsbsa(010)/
	retlw	B'00010100'	; scsbsa(011)/
	retlw	B'00101000'	; scsbsa(100)/
	retlw	B'00001001'	; scsbsa(101)/
	retlw	B'00100010'	; scsbsa(110)/
	retlw	B'00000000'	; invalid(111)

	END                       ; directive 'end of program'