;=========================================================================================
; System Name:	BLDC3_2
;
; File Name:	BLDC3_21.ASM 
;
; Description:	Implements sensorless control of BLDC motor with trapezoidal
;		Bemf.	
;
; Target Device:F243, F2407
; To Select the target device see x24x_app.h file.
;
;			
;				
;
; Last Update:	6-29-2000
; _________________________________________________________________________________________
; Date of Mod | 		    DESCRIPTION					
; ------------|----------------------------------------------------------------------------
;  6-29-00		Release REV 01     
;                                                     
;  10-24-00	This version is used for ASP and CMP1 motors
;==========================================================================================
;
;SYSTEM OPTIONS
;******************************************************************************************
real_time		.set	1	;set to 1 for real time mode, otherwise set 0

phase1_inc_build	.set	1
phase2_inc_build	.set	0
phase3_inc_build	.set	0
phase4_inc_build	.set	0
phase5_inc_build	.set	0

;select BLDC motor type 
wjw			.set	0
kvb			.set	0
aub			.set	0	
ads			.set	0
aBB			.set	0
asp			.set	1 
pxi			.set	0
cmp1		.set	0
                         
;------------------------------------------------------------------------------------------
; External references
;------------------------------------------------------------------------------------------

		.include	"x24x_app.h"

		.global 	MON_RT_CNFG
		.global		v_timer

		.ref	SYS_INIT

		.ref	DATA_LOG, DATA_LOG_INIT		;function call
		.ref	dlog_iptr1, dlog_iptr2		;Inputs
		.ref	trig_value                 

		.ref	DAC_VIEW_DRV, DAC_VIEW_DRV_INIT			;function call
		.ref	DAC_IPTR0, DAC_IPTR1, DAC_IPTR2, DAC_IPTR3 	;Inputs

		.ref	ADC04U_DRV, ADC04U_DRV_INIT			;function call
		.ref	A4_ch_sel,C1_gain,C2_gain,C3_gain,C4_gain	;inputs
		.ref	C1_out, C2_out, C3_out, C4_out			;Outputs	


		.ref	RMP2CNTL, RMP2CNTL_INIT				;function call
		.ref	rmp2_dly, rmp2_desired				;Inputs 
                .ref	rmp2_out    					;Output 
		.ref	rmp2_max, rmp2_min

		.ref	RMP3CNTL, RMP3CNTL_INIT				;function call
		.ref	rmp3_desired, rmp3_dly				;Inputs
		.ref	rmp3_out, rmp3_done_flg				;Outputs      
		.ref	rmp3_min

		.ref	IMPULSE, IMPULSE_INIT				;function call
		.ref	ig_period, ig_out				;Inputs 

		.ref	MOD6_CNT, MOD6_CNT_INIT				;function call
		.ref	m6_trig_in, m6_cntr				;Inputs

		.ref	BLDC_3PWM_DRV, BLDC_3PWM_DRV_INIT		;function call
		.ref	cmtn_ptr_bd, D_func, Mfunc_p	

		.ref	COMTN_TRIG, COMTN_TRIG_INIT			;function call
		.ref	Va, Vb, Vc, cmtn_trig, cmtn_ptr_ct	
		.ref	debug_Bemf, NW_DYN_THOLD
		.ref	cdnw_delta, noise_window_max    
		.ref	zc_trig, neutral, rev_period
		
		.ref	PID_REG2, PID_REG2_INIT				;function call
		.ref	pid_fb_reg2, pid_ref_reg2			;Inputs
		.ref	pid_out_reg2, 					;Output
		.ref	pid_max_reg2, pid_min_reg2		
		.ref	K0_reg2, K1_reg2, Kc_reg2

 		.ref	BC_INIT,BC_CALC		;function call
		.ref	BC_IN,BC_OUT		;Inputs/Outputs

        .ref	SPEED_REV_PRD, SPEED_REV_PRD_INIT		;function call
		.ref	event_period				;Input
		.ref	rpm_max, speed_scaler,shift		;parameter
		.ref	speed_prd, speed_rpm			;Outputs
;-------------------------------------------------------------------------------------------
; Local Variable Declarations
;-------------------------------------------------------------------------------------------

    .if (x243)    
;SYSTEM_INT_PERIOD	.set	1000		;50uS sampling period @50nS CPU clock
;SYSTEM_INT_PERIOD	.set	700		;17.5 uS sampling period @25nS CPU clock
;SYSTEM_INT_PERIOD	.set	360		;18 uS sampling period @50nS CPU clock
;SYSTEM_INT_PERIOD	.set	450		;22.5 uS sampling period @50nS CPU clock
SYSTEM_INT_PERIOD	.set	500		;25 uS sampling period @50nS CPU clock                    
	.endif
   
    .if (x2407)  
;SYSTEM_INT_PERIOD	.set	758		;25 uS sampling period @33nS CPU clock           
SYSTEM_INT_PERIOD	.set	1000	;25 uS sampling period @25nS CPU clock           
	.endif  
		
		
		.def	GPR0			
		.def	GPR1
		.def	GPR2
		.def	v_timer
		.def	align_flag
            
		.bss	GPR0,1			
		.bss	GPR1,1
		.bss	GPR2,1
		.bss	v_timer,1   
		.bss	current_set,1
		.bss	GPR3,1    		
		.bss	I_loop_flg,1
		
        .bss	D_func_desired,1	
		.bss	align_flag,1	  

        .bss	cmtn_period_target,1
		.bss	cmtn_period_setpt,1 
        .bss	sp_up_done_flg,1 
		.bss	ramp_delay,1    
		.bss	loop_cnt,1
        .bss	isr_ticker,1
		
;===========================================================================================
; V E C T O R    T A B L E    ( including RT monitor traps )
;===========================================================================================
                 .include "c200mnrt.i"

		.sect "vectors"  
		.def 	_c_int0

RESET	    B	  _c_int0 	 	; 00
INT1	    B	  PHANTOM	 	; 02
INT2	    B	  PHANTOM	 	; 04
INT3	    B	  T2_PERIOD_ISR ; 06
INT4	    B	  PHANTOM	 	; 08
INT5	    B	  PHANTOM	 	; 0A
INT6	    B	  PHANTOM	 	; 0C



		.include	"rtvecs.h"

; Note : The above include line must be AFTER the user configurable 
;        vectors. Do not change the place where this line is included.



;==========================================================================================
; M A I N   C O D E  - starts here
;==========================================================================================
		.text
_c_int0:
		CALL	SYS_INIT
		CALL	DAC_VIEW_DRV_INIT
        CALL    DATA_LOG_INIT	
		CALL	BLDC_3PWM_DRV_INIT
		CALL	IMPULSE_INIT
		CALL	MOD6_CNT_INIT
		CALL	ADC04U_DRV_INIT      
		CALL	RMP3CNTL_INIT	
		CALL	COMTN_TRIG_INIT
		CALL	PID_REG2_INIT 
       	CALL    RMP2CNTL_INIT
        CALL    BC_INIT
        CALL    SPEED_REV_PRD_INIT
;------------------------------------------------------------------------------------------
;System time-base init
;------------------------------------------------------------------------------------------
;	Here time base is derived from T1 Underflow Int (i.e. Period)
;	in BLDC_3PWM_DRV module.

	;Initialize period register
		POINT_EV
		SPLK	#SYSTEM_INT_PERIOD, T2PER

			;5432109876543210
			;||||!!!!||||!!!!
		SPLK	#1001000001000000b, T2CON  	;Asym
		
;------------------------------------------------------------------------------------------
; Initialise the Real time monitor
;---Real Time enable---------------
	.if (real_time)
		CALL	MON_RT_CNFG			;For Real-Time
	.endif
;------------------------------------------------------------------------------------------

;------------------------------------------------------------------------------------------
; System Interrupt Init.
;------------------------------------------------------------------------------------------
	;Event Manager
		POINT_EV
;		SPLK	#0000001000000000b,IMRA ;Enable T1 Underflow Int (i.e. Period)
		SPLK	#0000000000000100b,IMRB ;Enable T2 Underflow Int (i.e. Period)
;        SPLK	#0000000000000001b,IMRB
;		SPLK	#0000000000000100b,IMRC ;Enable CAP3 int (i.e. QEP index pulse)
			;||||!!!!||||!!!!		
			;5432109876543210

		SPLK	#0FFFFh,IFRA	; Clear all Group A interrupt flags
		SPLK	#0FFFFh,IFRB	; Clear all Group B interrupt flags
		SPLK	#0FFFFh,IFRC	; Clear all Group C interrupt flags

	;C2xx Core
		POINT_PG0

;---Real Time enable------------------------------------------------------------------------
	.if (real_time)
;		SPLK	#0000000001000010b,IMR	;En Int lvl 2 & 7 (T1 ISR)
		SPLK	#0000000001000100b,IMR	;En Int lvl 3 & 7 (T2 ISR)
			;5432109876543210
	.endif


	.if (real_time != 1)
;		SPLK	#0000000000000010b,IMR	;En Int lvl 2 (T1 ISR)
		SPLK	#0000000000000100b,IMR	;En Int lvl 3 (T2 ISR)
			;||||!!!!||||!!!!		
			;5432109876543210
	.endif

		SPLK	#0FFFFh, IFR		;Clear any pending Ints
		EINT				;Enable global Ints
;------------------------------------------------------------------------------------------
;Hardware/Board Specific Initialization
;------------------------------------------------------------------------------------------
;-----------------------------------------------------------------------
;Enables PWM signals on DMC1500  
;-----------------------------------------------------------------------
	.if (x243|x2407)		;target dependancy
		POINT_PF2
		LACC	OCRA
		AND		#0BFFFh	
		SACL	OCRA			;Select Secondary function IOPB6

		LACC	PBDATDIR
		OR		#04000h
		SACL	PBDATDIR		;Set IOPB6 as output

		LACC	PBDATDIR
		AND		#0FFBFh     	;Set IOPB6 low, Enable PWM
;       OR		#00040h     	;Set IOPB6 high, Disable PWM
		SACL	PBDATDIR  	
	.endif

;Selects ADC Channels

		ldp	#A4_ch_sel  
	.if (x2407)     
;		SPLK	#0cba5h, A4_ch_sel	;Chs 12,11,10,5 for 2407EVM with DMC1500                    
		SPLK	#06543h, A4_ch_sel	;Chs 6,5,4,3 for 2407eZdsp with DMC1500                    
	.endif
   
    	.if (x243)  
    		SPLK	#06543h, A4_ch_sel	;Chs 6,5,4,3 for 243EVM with DMC1000             
	.endif  
		
         
;------------------------------------------------------------------------------------------
;Motor Specific Parameters Initialization
;------------------------------------------------------------------------------------------
;Compressor motor 1
;------------------------------------
	.if (cmp1)
ALIGN_DUTY	.set	0F00h   
LOOP_CNT_MAX	.set	0

		POINT_B0
;For open loop speed ramp control
		SPLK	#0400h, cmtn_period_target	;Final commutation period
		SPLK	#500h, cmtn_period_setpt	;Initial commutation period 
		
        SPLK	#0100, ramp_delay                   
    
;For open loop D_func control	    
		SPLK	#0h, I_loop_flg			;start with open loop current
		SPLK	#ALIGN_DUTY, D_func_desired

	.endif 

;Motor pac scientific
;------------------------------------
	.if (pxi)
ALIGN_DUTY	.set	0F00h   
LOOP_CNT_MAX	.set	0

		POINT_B0
;For open loop speed ramp control
		SPLK	#0450h, cmtn_period_target	;Final commutation period
		SPLK	#500h, cmtn_period_setpt	;Initial commutation period 
		
        SPLK	#050, ramp_delay                   
    
;For open loop D_func control	    
		SPLK	#0h, I_loop_flg			;start with open loop current
		SPLK	#ALIGN_DUTY, D_func_desired

	.endif 
	
;Motor asp
;-----------------------------------
	.if (asp)
ALIGN_DUTY	.set	03000h   
LOOP_CNT_MAX	.set	8       ;
;LOOP_CNT_MAX	.set	2

		POINT_B0
;For open loop speed ramp control
;		SPLK	#0500h, cmtn_period_target	;Final ramp speed (lower val = higher speed)
;		SPLK	#600h, cmtn_period_setpt	;Init ramp starting point
;        SPLK	#050, ramp_delay      ;For LF2407
        
        SPLK	#0700h, cmtn_period_target	;Final ramp speed (lower val = higher speed)
		SPLK	#0C00h, cmtn_period_setpt	;Init ramp starting point
        SPLK	#0200, ramp_delay
        
;For open loop D_func control
		SPLK	#0h, I_loop_flg			;start with open loop current
 		SPLK	#ALIGN_DUTY, D_func_desired	;set PWM duty cycle

	.endif  
	
;Motor wjw
;-----------------------------------
	.if (wjw)
ALIGN_DUTY	.set	01500h    
LOOP_CNT_MAX	.set	0

		POINT_B0
;For open loop speed ramp control
		SPLK	#0300h, cmtn_period_target	;Final ramp speed (lower val = higher speed)
		SPLK	#500h, cmtn_period_setpt	;Init ramp starting point
		SPLK	#0400, ramp_delay		;Set ramp rate (higher val = slower ramp)

;For open loop D_func control
		SPLK	#0h, I_loop_flg			;start with open loop current
 		SPLK	#ALIGN_DUTY, D_func_desired	;set PWM duty cycle

	.endif  
	
;Motor kvb
;-----------------------------
	.if (kvb)
ALIGN_DUTY	.set	0C00h  
LOOP_CNT_MAX	.set	1

		POINT_B0
;For open loop speed ramp control
        SPLK	#0200h, cmtn_period_target	;Final ramp speed
		SPLK	#500h, cmtn_period_setpt	;Init ramp starting point
	 
		SPLK	#0400, ramp_delay		;Set ramp rate (higher val = slower ramp)

;For open loop D_func control
		SPLK	#0h, I_loop_flg			;start with open loop current
        SPLK	#ALIGN_DUTY, D_func_desired
 
	.endif

;Motor aub
;-----------------------------------
	.if (aub)
ALIGN_DUTY	.set	0F00h 
LOOP_CNT_MAX	.set	0

		POINT_B0
;For open loop speed ramp control
		SPLK	#0200h, cmtn_period_target	;Final ramp speed (lower val = higher speed)
		SPLK	#500h, cmtn_period_setpt	;Init ramp starting point
		SPLK	#0400, ramp_delay		;Set ramp rate (higher val = slower ramp)

;For open loop D_func control
		SPLK	#0h, I_loop_flg			;start with open loop current
 		SPLK	#ALIGN_DUTY, D_func_desired	;set PWM duty cycle

	.endif


;Motor ads
;------------------------------------
	.if (ads)
ALIGN_DUTY	.set	0F00h   
LOOP_CNT_MAX	.set	0

		POINT_B0
;For open loop speed ramp control
		SPLK	#0300h, cmtn_period_target	;Final commutation period
		SPLK	#500h, cmtn_period_setpt	;Initial commutation period 
		
	.if (x243)
        SPLK	#0200, ramp_delay   			;Set ramp rate                 
	.endif
   
    	.if (x2407)
        SPLK	#01000, ramp_delay                   
	.endif  
    
;For open loop D_func control	    
		SPLK	#0h, I_loop_flg			;start with open loop current
		SPLK	#ALIGN_DUTY, D_func_desired

	.endif         
	
           
;------------------------------------------------------------------------------------------
;Other Parameters Initialization
;------------------------------------------------------------------------------------------
        POINT_B0
		SPLK	#0, sp_up_done_flg	
		SPLK	#0Fh, align_flag		
		SPLK	#0,v_timer			;set virtual timer to start at zero.
        SPLK	#0,loop_cnt        
;------------------------------------------------------------------------------------------
;Initial parameter passing to modules
;------------------------------------------------------------------------------------------		
		ldp	#rmp2_out
		SPLK	#ALIGN_DUTY, rmp2_out
		SPLK	#50h, rmp2_dly
		SPLK	#07fffh, rmp2_max	
		SPLK	#0fh, rmp2_min	
	
		ldp	#rmp3_desired
		bldd	#cmtn_period_target, rmp3_desired             	
        bldd	#ramp_delay, rmp3_dly	
        bldd	#cmtn_period_setpt, rmp3_out
		SPLK	#50h, rmp3_min   	
		
		ldp	#D_func
		SPLK	#ALIGN_DUTY, D_func
		
		ldp	#NW_DYN_THOLD
		splk	#15, NW_DYN_THOLD	
		SPLK	#2, cdnw_delta		
		LACC	NW_DYN_THOLD
		SUB	cdnw_delta
		SACL	noise_window_max
   
	    ldp	#pid_out_reg2
		splk	#7000h, pid_max_reg2	;Q15