;******************************************************************************
;
; Software License Agreement                                         
;                                                                    
; The software supplied herewith by Microchip Technology             
; Incorporated (the "Company") is intended and supplied to you, the  
; Company抯 customer, for use solely and exclusively on Microchip    
; products. The software is owned by the Company and/or its supplier,
; and is protected under applicable copyright laws. All rights are   
; reserved. Any use in violation of the foregoing restrictions may   
; subject the user to criminal sanctions under applicable laws, as   
; well as to civil liability for the breach of the terms and         
; conditions of this license.                                        
;                                                                     
; THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES,  
; WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED  
; TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A       
; PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT,  
; IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR         
; CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.       
;
;*********************************************************************************
;-----------------------------------------------------------------
;This program is used for sensorless control of BLDC motor.
;Hardwrae used is PICDEM MC LV board.
;Motor used for testing is NTDynamo Hurst motor. 
;Theory and implementation is expalined in application note AN970
;Motor connections to the board is as follows:
;Motor power connection(White square connector with 4 wires)
;Red : connect to M1 on J9 on the board 
;White : connect to M2 on J9 on the board 
;Black : connect to M3 on J9 on the board 
;Green : connect to G on J9 on the board 
;-----------------------------------------------------------------
;	Author	: Padmaraja Yedamle
;			: Home Appliance Solutions Group
;			: Microchip Technology Inc
;	Date	: June-25-2004
;
;******************************************************************
	include 	"snsrles_DATA.inc"
;--------------------------------------------------
;Configuration register settings

	LIST p=18f2431,f=INHX32

	
	__CONFIG _CONFIG1H, 0x06 ;_OSC_HS_1H &_FCMEN_OFF_1H&_IESO_OFF_1H
	__CONFIG _CONFIG2L, 0x0E ;_PWRTEN_ON_2L & _BOREN_ON_2L & _BORV_20_2L  
	__CONFIG _CONFIG2H, 0x1E ;_WDTEN_OFF_2H
	__CONFIG _CONFIG3L, 0x3C ;0x24 ;_PWMPIN_OFF_3L & _LPOL_LOW_3L & _HPOL_LOW_3L & _GPTREN_ON_3L
	__CONFIG _CONFIG3H, 0x9D ;_FLTAMX_RC1_3H & _PWM4MX_RB5_3H
	__CONFIG _CONFIG4L, 0x80 
	__CONFIG _CONFIG5L, 0x0F 
	__CONFIG _CONFIG5H, 0xC0  
	__CONFIG _CONFIG6L, 0x0F 
	__CONFIG _CONFIG6H, 0xE0 
	__CONFIG _CONFIG7L, 0x0F 
	__CONFIG _CONFIG7H, 0x40    
;--------------------------------------------------




;******************************************************************
#define		HURST_MOTOR	
;#define 	PICDEM_MC_LV_SCHEMATIC
#define		AN970_SCHEMATIC


#define	CYCLE_COUNT_MAXH	0x4E
#define	CYCLE_COUNT_MAXL	0x20
#define	MAX_FLTA_COUNT	0x20
#define	MAX_FLTB_COUNT	.20
#define	MAX_HEATSINKTEMP	.20

#define CURRENT_FAULT_INPUT	1
;----------------------------------------------
;FLAGS bits
#define	HALL_FLAG		0
#define	FLAG_FAULT		1
#define	PARAM_DISPLAY 	2
#define	POSITION_BIT 	3
#define	VELOCITY_READY 	4
#define	NEGATIVE_ERROR 	5
#define CALC_PWM		6


;FLAGS1 bits
#define	DEBOUNCE	0
#define	KEY_RS		1
#define	KEY_FR		2
#define	KEY_PRESSED 3

#define	RUN_STOP 4
#define	FWD_REV	5


;FLT_FLAGS bits
#define	OCUR	0
#define	OVOLT	1
#define	OTEMP	2

;FLAGS_SRLS bits
#define	IC_ALOWED	0
#define	SWITCH_SENSORLESS	1
#define PHASE_SHIFT_READY	2
#define	OPEN_SPEED_STEP2	3

#define	PC_COM	7

;Delay parameters
#define	DELAY_COUNT1	0x01
#define	DELAY_COUNT2	0xFF

#ifdef	PICDEM_MC_LV_SCHEMATIC
;Keys parameters
#define KEY_PORT PORTC
#define RUN_STOP_KEY 0
#define FWD_REV_KEY 2
#define DEBOUNCE_COUNT 0x8F
;LED parameters
#define LED_PORT PORTC
#define RUN_STOP_LED 5
#define FWD_REV_LED 4

#define	LED1	PORTC,5
#define	LED2	PORTC,4
#define	LED3	PORTC,3
#endif

#ifdef	AN970_SCHEMATIC
;Keys parameters
#define KEY_PORT PORTC
#define RUN_STOP_KEY 4
#define FWD_REV_KEY 5
#define DEBOUNCE_COUNT 0x8F
;LED parameters
#define LED_PORT PORTC
#define RUN_STOP_LED 0
#define FWD_REV_LED 2

#define	LED1	PORTC,0
#define	LED2	PORTC,2
#define	LED3	PORTC,3
#endif

;******************************************************************
BLDC_MOTOR_CONTROL	UDATA_ACS

TEMP			res	1
TEMP1			res	1
HALL_SENSOR_COUNT	res	1
SPEED_REFH			res	1
SPEED_REFL			res	1
FLAGS				res	1
FLAGS1				res	1
FLT_FLAGS			res	1
FLAGS_SRLS			res	1

DEBOUNCE_COUNTER	res	1
COUNTER			res	1
COUNTER1			res	1
COUNTER_SP			res	1
COUNTER_SP1			res	1
RPM_COUNTER			res	1
VELOCITY_READH		res	1
VELOCITY_READL		res	1

SPEED_REF_RPMH		res	1
SPEED_REF_RPML		res	1
SPEED_FEEDBACKH		res	1
SPEED_FEEDBACKL		res	1
SPEED_ERRORH		res	1
SPEED_ERRORL		res	1
ERROR_PWMH			res	1
ERROR_PWML			res	1

POSITION_TABLE_FWD	res	8
POSITION_TABLE_REV	res	8

CURRENT_UH			res	1
CURRENT_UL			res	1

PDC_TEMPH			res	1
PDC_TEMPL			res	1

ARG1H				res	1
ARG1L				res	1
ARG2H				res	1
ARG2L				res	1
RESH				res	1
RESL				res	1

CYCLE_COUNTH		res	1
CYCLE_COUNTL		res	1

FAULTA_COUNT		res	1
FAULTB_COUNT		res	1
PWM_CYCLE_COUNT		res	1
OPEN_HALL			res	1
TABLE_OFFSET		res	1
COUNT_OPEN_LOOPL	res	1
COUNT_OPEN_LOOPH	res	1

DISPLAY_TEMP1		res	1
DISPLAY_TEMP2		res	1

OVDCOND_TEMP		res	1
BEMF_ZC				res	1
LOCK_ROTOR_COUNT	res	1

;----------------------------------------------------------------
STARTUP	code 0x00
 	goto	Start		;Reset Vector address 
	
	CODE	0x08
	goto	ISR_HIGH	;Higher priority ISR at 0x0008

PRG_LOW	CODE	0x018
	goto	ISR_LOW		;Lower priority ISR at 0x0018
	
;****************************************************************
PROG	code
Start
;****************************************************************
	clrf	PDC_TEMPH	
	clrf	PDC_TEMPL		
	clrf	SPEED_REFH		
	clrf	FLAGS
	clrf	FLAGS1
	
	call	FIRST_ADC_INIT				;Initialize ADC

WAIT_HERE								;Wait here till a key is pressed
	call	LED_BLINK
	call	KEY_CHECK
	btfss	FLAGS1,KEY_PRESSED
	bra		WAIT_HERE
	call	INIT_PERPHERALS				; Initialize all required peripherals(ADC,PWM,IC etc)

	
	bcf		LED1
	bcf		LED2
	bcf		LED3

	movlw	0xA0
	movwf	COUNTER_SP1

	
	clrf	FLAGS
	clrf	FLAGS1
	clrf	FLT_FLAGS
	clrf	FLAGS_SRLS
	movlw	0xF0
	movwf	COUNT_OPEN_LOOPH
	movwf	COUNT_OPEN_LOOPL
		
	bsf		INTCON,PEIE			;Port interrupts enable
	bsf		INTCON,GIE			;Global interrupt enable

	bsf		FLAGS1,FWD_REV
	clrf	OVDCOND_TEMP
;*******************************************************************
;The control loops in this routine
MAIN_LOOP
	btfss	FLAGS_SRLS,SWITCH_SENSORLESS	;Is the control ready to switch to sensoless?			
	bra		KEEP_SAME_PWM					;No; ramp up is in progress
	btfss	FLAGS,CALC_PWM					;
	bra		KEEP_SAME_PWM	
	call	UPDATE_PWM						;Reload PWM duty cycles according to the speed required
	bcf		FLAGS,CALC_PWM

KEEP_SAME_PWM	
	call	KEY_CHECK						;Any key pressed?
	call	PROCESS_KEY_PRESSED				;Service Key routine

	btfsc	FLT_FLAGS,OCUR					;Over current condition?
	call	FAULTA_PROCESS					;Stop motor and stand by

	btfsc	FLAGS,FLAG_FAULT				;If the fault exists, toggle the LEDs
	call	TOGGLE_LEDS
	
	btfss	ADCON0,GO	
	bsf		ADCON0,GO	;Set GO bit for ADC conversion start	

	goto	MAIN_LOOP
;--------------------------------------------------------------
;Higher priority Interrupt Service routine 
;In this routine, activities on Input capture pins are monitored.
;Back EMF signals are compared with a DC voltage in the hardware. 
; Based on the BEMF states, the switchig sequence is changed.
;--------------------------------------------------------------

ISR_HIGH
	btfsc	FLAGS_SRLS,IC_ALOWED		;Check if the control switched to sensorless
	bra		BYPASS_IC_INT				;No
	btfsc	PIR3,IC1IF					;Yes, monitor Input capure pin1
	bra		HALL_A_HIGH

BYPASS_IC_INT
	btfsc	PIR1,TMR1IF					;Timer1 overflow interrupt
	bra		TIMER1_INT

	btfsc	PIR1,ADIF					;ADC interrupt
	bra		AD_CONV_COMPLETE	

	btfsc	PIR3,PTIF					;PWM interrupt
	bra		PWM_INTERRUPT
	
	RETFIE	FAST

;******************************************************************
AD_CONV_COMPLETE			;ADC interrupt
	
	movff	ADRESL,CURRENT_UL	;Sample A = Iu
	movff	ADRESH,CURRENT_UH

	movff	ADRESL,SPEED_REFL	;Sample B = speed ref
	movff	ADRESH,SPEED_REFH	
	
	bsf		FLAGS,CALC_PWM
	bcf		PIR1,ADIF		;ADIF flag is cleared for next interrupt
	RETFIE	FAST		

;******************************************************************
HALL_A_HIGH
	bcf		PIR3,IC1IF
	clrf	LOCK_ROTOR_COUNT
	call	CHECK_SEQUENCE		;UPDATE_SEQUENCE		;Update the commutation sequence 
	RETFIE	FAST

;-----------------------------------------
;This routine accelerates the motor speed in two levels.
;In first level, the speed is set to 450RPM, open loop.
;In second level it is acceleated to 800RPM openloop.
;Then the BEMF signals are monitored for 256 cycles and the switching sequence is synched with BEMF signals.
;-----------------------------------------
CHECK_SEQUENCE
	bsf		FLAGS_SRLS,PC_COM

	btfss	FLAGS_SRLS,OPEN_SPEED_STEP2		
	return
	btfsc	FLAGS_SRLS,SWITCH_SENSORLESS		
	bra		SENSERLESS_ON
	incfsz	COUNT_OPEN_LOOPH,F
	return
	bsf		FLAGS_SRLS,SWITCH_SENSORLESS
	movlw	b'00111010'
	movwf	DFLTCON
	return
SENSERLESS_ON
	bsf		FLAGS_SRLS,IC_ALOWED
	bcf		PIE3,IC1IE		;Cap1 interrupt


	movlw	0xEA
	cpfsgt	VELOCITY_READH,W		;Digital filters are programmed
	bra		LOWER_SPEED_LEVEL1		;at 3 levels
	movlw	b'00111001'
	movwf	DFLTCON
	bsf		LED2
	bra		CONTINUE_SPEED
LOWER_SPEED_LEVEL1
	movlw	0xE4
	cpfsgt	VELOCITY_READH,W
	bra		LOWER_SPEED_LEVEL2
	movlw	b'00111101'
	movwf	DFLTCON
	bcf		LED2
	bra		CONTINUE_SPEED
LOWER_SPEED_LEVEL2
	movlw	b'00111110'
	movwf	DFLTCON
	btg		LED2
CONTINUE_SPEED
	movff	VELOCITY_READH,TMR1H
	movff	VELOCITY_READL,TMR1L

	bsf	PIE1,TMR1IE


	btg		LED3


	return

;-------------------------------------------------------------
;The switching sequence is updated in this routine
UPDATE_SEQUENCE
;init table again
	btfss	FLAGS1,FWD_REV
	bra		ITS_REVERSE
	lfsr	0,POSITION_TABLE_FWD
	bra		PICK_FROM_TABLE
ITS_REVERSE
	lfsr	0,POSITION_TABLE_REV
;--
PICK_FROM_TABLE
	movf	PORTA,W
	comf	WREG,W		;In forward direction, the BEMF ZC is inverted
	andlw	0x1C		;IC1/IC2/IC3
	rrncf	WREG,W
	rrncf	WREG,W
	movwf	BEMF_ZC
	movf	PLUSW0,W
	movwf	OVDCOND_TEMP	
	return
;-------------------------------------------------------------
;Timer1 overflow interrupt
TIMER1_INT
	bcf		PIR1,TMR1IF
	btfsc	FLAGS_SRLS,SWITCH_SENSORLESS		
	bra		SENSERLESS_T0INT
OPEN_LOOP_ACTIVE

	bcf		FLAGS_SRLS,IC_ALOWED	
	btfsc	FLAGS_SRLS,OPEN_SPEED_STEP2
	bra		OPEN_SPEED_STEP_2
	movlw	LOW_OL_SPEEDH		;0xD8F0 = 1.92mSec@20MHz/1000RPM
	movwf	TMR1H
	movlw	LOW_OL_SPEEDL
	movwf	TMR1L	
	call	SET_SEQUENCE_COUNT
	infsnz	COUNT_OPEN_LOOPL,F
	bsf		FLAGS_SRLS,OPEN_SPEED_STEP2
	retfie	FAST
OPEN_SPEED_STEP_2
	movlw	HIGH_OL_SPEEDH	;B0		;0xD8F0 = 1.92mSec@20MHz/1000RPM
	movwf	TMR1H
	movlw	HIGH_OL_SPEEDL	;F0
	movwf	TMR1L	
	call	SET_SEQUENCE_COUNT
	retfie	FAST


SENSERLESS_T0INT
	bcf		FLAGS_SRLS,IC_ALOWED	

	bsf		FLAGS_SRLS,SWITCH_SENSORLESS		
	call	UPDATE_SEQUENCE
	movff	OVDCOND_TEMP,OVDCOND

	movff	VELOCITY_READH,TMR1H
	movff	VELOCITY_READL,TMR1L

BLOCK_IC_OVER
	bsf		PIE3,IC1IE		;Cap1 interrupt
	bsf		PIE1,TMR1IE

	retfie	FAST

;******************************************************************
ISR_LOW

	RETFIE			

;******************************************************************
;Check overcurrent fault in this routine for 'n' number of PWM cycles in one Timer1 cycle,
;and if the fault persists, the input type is changed to catestrophic mode.
;
PWM_INTERRUPT
	bcf		PIR3,PTIF
	infsnz	LOCK_ROTOR_COUNT,F
	clrf	OVDCOND

CHECK_OTHER_FAULTS
	incfsz	PWM_CYCLE_COUNT,F
	bra		CHECK_FOR_FAULTS
	clrf	FAULTA_COUNT
	bra		CHECK_PARAMETER_DISPLAY	
	
CHECK_FOR_FAULTS
	btfsc	PORTC,CURRENT_FAULT_INPUT
	bra		CHECK_PARAMETER_DISPLAY	
	incf	FAULTA_COUNT,F
	movlw	MAX_FLTA_COUNT
	cpfsgt	FAULTA_COUNT
	bra		CHECK_PARAMETER_DISPLAY	
	bcf		FLTCONFIG,FLTAMOD
	bsf		FLTCONFIG,FLTAEN
	bsf		FLT_FLAGS,OCUR
;----------------------------------Old code
CHECK_PARAMETER_DISPLAY
	retfie	FAST
	movlw	CYCLE_COUNT_MAXH
	cpfseq	CYCLE_COUNTH
	bra		NOT_YET_THERE
	movlw	CYCLE_COUNT_MAXL
	cpfsgt	CYCLE_COUNTL
	bra		NOT_YET_THERE
	bsf		FLAGS,PARAM_DISPLAY
	clrf	CYCLE_COUNTH
	clrf	CYCLE_COUNTL
	btfsc	FLT_FLAGS,OCUR
	btg		LED1
	bcf		PIR3,PTIF
	retfie	FAST
NOT_YET_THERE
	incfsz	CYCLE_COUNTL,F
	retfie	FAST
	incf	CYCLE_COUNTH,F
	bcf		PIR3,PTIF
	retfie	FAST

;******************************************************************
;PWM duty cycle is updated according to the potentiometer level
UPDATE_PWM
	movlw	0x40	;Check lower limit for PWM
	cpfsgt	SPEED_REFH	
	bra		RESET_DUTY_CYCLE

	movlw	0x40	;Add offset to the speed ref
	addwf	SPEED_REFH,F
	btfss	STATUS,C
	bra		SPEED_REF_OK
	movlw	0xFF
	movwf	SPEED_REFH
SPEED_REF_OK	
	movlw	0xFD				;Check upper limit to the speed 
	cpfslt	SPEED_REFH	
	movwf	SPEED_REFH

	call	CALCULATE_TIME_60DEG

;PWM = [(MotorVoltage/DCbus voltage)*(PTPER*4)]*[SpeedRef/255] *16
;16 is the multiplication factor
	movf	SPEED_REFH,W	
	mullw	(MAIN_PWM_CONSTANT)
	swapf	PRODL,W
	andlw	0x0F
	movwf	PDC_TEMPL
	swapf	PRODH,W
	andlw	0xF0
	iorwf	PDC_TEMPL,F
	swapf	PRODH,W
	andlw	0x0F
	movwf	PDC_TEMPH
 
	bsf		PWMCON1,UDIS	;Disable the PWM buffer update

	movf	PDC_TEMPH,W
	movwf	PDC0H
	movwf	PDC1H
	movwf	PDC2H	
	movwf	PDC3H	

	movf	PDC_TEMPL,W
	movwf	PDC0L	
	movwf	PDC1L
	movwf	PDC2L
	movwf	PDC3L

	bcf		PWMCON1,UDIS	;Disable the PWM buffer update

	RETURN



;---------------------------------------------
RESET_DUTY_CYCLE
	clrf	PDC0H
	clrf	PDC1H
	clrf	PDC2H
	clrf	PDC3H	
	clrf	PDC0L
	clrf	PDC1L
	clrf	PDC2L
	clrf	PDC3L

	RETURN

;---------------------------------------------
;This routine calculates a 60 electrical deg delay

CALCULATE_TIME_60DEG	
	movlw	0x63
	subwf	SPEED_REFH,W	;offset
	mullw	0x9D	;39.2*4
	bcf		STATUS,C
	rrcf	PRODH,F		;/4
	rrcf	PRODL,F				
	bcf		STATUS,C
	rrcf	PRODH,F
	rrcf	PRODL,F				
;FFFF - (10,000 - 39.2 X (ADC-64h))
	bcf		STATUS,C
	movlw	0xEF
	addwfc	PRODL,F