📄 bl_esc_100a_307b.asm
字号:
jmp GET_CMP_DATA
DEC_PWR: ;mov a, ON_DUTY ; Test monitor only
dec ON_DUTY
;-----------------
GET_CMP_DATA:
clr EA
call GET_COMP_DATA ; Compare the motor generative voltage while drive 3-ST timing.
setb EA
;13uS from OFF_TIMER end with stick midium.
mov a, ROUND_CHK_FLAG ;
anl a, #11111100b ;
cjne a, #11111100b, COMP_STATE1 ; spin with all state sequence?
call WDT_OFF ; Yes. Spinning check by WDT for MOSFET protection
mov a, ROUND_CHK_FLAG ; Round check for WDT power off protection control
anl a, #00000011b ; Clear the spinning status
mov ROUND_CHK_FLAG, a ;
cjne a, #3, COMP_STATE1_3SPINS ; a few times run round in BEMF mode?
BACK_TO_BEMF: mov ROUND_CHK_FLAG, #0 ; reset the round check flag
jmp COMP_STATE1 ;
COMP_STATE1_3SPINS: mov a, ROUND_CHK_FLAG
inc a
mov ROUND_CHK_FLAG, a
COMP_STATE1: cjne COMP_D, #1, COMP_STATE2
orl ROUND_CHK_FLAG, #00000100b ; Spin check flag to make sure the starting
call STATE1
jmp BEMF_DRIVE_ON
COMP_STATE2: cjne COMP_D, #2, COMP_STATE3
orl ROUND_CHK_FLAG, #00001000b
call STATE2
jmp BEMF_DRIVE_ON
COMP_STATE3: cjne COMP_D, #3, COMP_STATE4
orl ROUND_CHK_FLAG, #00010000b
NORMAL_RUN:
COMP_STATE_03: call STATE3
jmp BEMF_DRIVE_ON
COMP_STATE4: cjne COMP_D, #4, COMP_STATE5
orl ROUND_CHK_FLAG, #00100000b
call STATE4
jmp BEMF_DRIVE_ON
;
COMP_STATE5: cjne COMP_D, #5, COMP_STATE6
orl ROUND_CHK_FLAG, #01000000b
BEMF_DRIVE_5: call STATE5
jmp BEMF_DRIVE_ON
;
COMP_STATE6: ;cjne COMP_D, #6, COMP_STATE_STOP_OVER ; COMP_D =7
DRIVE_6: orl ROUND_CHK_FLAG, #10000000b
call STATE6
jmp BEMF_DRIVE_ON
;COMP_STATE_STOP_OVER: ; COMP_D =7
;jmp GET_BEMF ; Time over when motor stops and the comparator can work properly
BEMF_DRIVE_ON: ;26uS from OFF_TIMER end with stick midium.
; <<<<<<<<
mov a, SAMPLING_COUNTER_L ; inc number of sampling counts
clr c
add a, #1
mov SAMPLING_COUNTER_L, a
mov a, SAMPLING_COUNTER_H
addc a, #0
mov SAMPLING_COUNTER_H, a
call ON_TIMER
;call CHECK_TOP_CELL_VOLTAGE ; voltage down and over current check routine
BEMF_MAX_THROTTLE_CHK1: mov a, ON_DUTY ; May be ON_DUTY is 100 at low voltage driving
clr c
subb a, BEMF_FULL_ON ; Is it full throttle drive condition?
jnc ZERO_CROSS_IN ; Yes, start zero cross drive
jmp GET_BEMF
;---------------------------------------------------
GET_BEMF1: call SPIN_FREE
mov ROUND_CHK_FLAG, #11111100b ; spin all sequence
mov ON_DUTY, BEMF_FULL_ON
dec ON_DUTY ; -1
mov DRV_SEQUENCE_CHECK_ENABLE, #0 ; Not sure the first spin state at here
mov RX_NOISE_FILTER_COUNTER, #0
mov ACCELERATION_CKECK_AVERAGE_FILTER_COUNTER, #127 ; Checking BEMF max speed
mov SPIN_MODE, #1 ; set to BEMF mode
mov SPIN_COUNTER, #5 ; check slow down with this value spins
TWICE_SAME_SAMPLING_DATA_CHECK: ;<<<<<<<<
call START_BEMF_SAMPLING_COUNTER
call GET_COMP_DATA
mov TEMP_MEMORY, COMP_D
;
call WDT_OFF
CHECK_BEMF_SAMPLING_TIME_END:
mov a, TMR3CN ; BEMF_SAMPLING_TIME_END?
anl a, #10000000b ; cheking timer3 over flow flag high
jnz TWICE_SAME_SAMPLING_DATA_CHECK ; time over
call GET_COMP_DATA
mov a, COMP_D
clr c
subb a, TEMP_MEMORY
jz CHECK_BEMF_SAMPLING_TIME_END ; Still same BEMF data
; MAke sure slow enough for sampling as BEMF spin?
SLOW_DOWN_CHECK_WITH_SOME_SPINS:
dec SPIN_COUNTER
mov a, SPIN_COUNTER
jnz TWICE_SAME_SAMPLING_DATA_CHECK
WAIT_SET_BEMF_RUN: mov SPIN_COUNTER, #20
;setb p0.6 ; MONITOR
call START_T3
jmp GET_BEMF
;---------------------------------------------------
ZERO_CROSS_IN:
;setb p0.6 ; MONITOR
;
BEMF_ACCELERATION_MONITOR:
mov a, ACCELERATION_CKECK_AVERAGE_FILTER_COUNTER
clr c
subb a, #127 ; Cheking the acceleration average + or -
jc THROTTLE_OFF_CHK3
;setb p0.6 ; MONITOR
jmp GET_BEMF
THROTTLE_OFF_CHK3:
;clr p0.6 ; MONITOR
mov TEMP3, CONTROL_DATA ; Get Rx input pulse to check stop
;------------------------------
ZERO_CROSS_RUN: mov SPIN_MODE, #0 ; set to Zero Cross spin mode
;Accelerate timer makes inc/dec power calc only for power control.
mov SPIN_COUNTER, #1 ; stay 50% power run 254 spins at zero cross mode starting by this count
; advance timing should be low to get near 50% power at mode changing.
call SPIN_FREE ; turn off sysc. rectifier for GET_COMP_DATA
STATE_CHECK_3:
ZERO_C4:
;** IF SOMETHING HAPPENED HERE, THEN SYSTEM STOPS BY WATCH DOG TIMER!!! **
clr EA
call GET_COMP_DATA
setb EA
cjne COMP_D, #4, ZERO_C4; Get start point of state4
mov PREV_DRIVE_STATE, #3
call STATE4 ; U=3ST, V=1, W=0 -> U=0, V=1, W=3ST
mov CPT0MX, #01010010b ; CP in-: P1.3(HALFW), CP in+: P0.4(W) with UP slope
;call INDUCKTIVE_KICK_DET_LH ; NO INDUCTIVE KICK.Then Start T2
call START_T2
call ZERO_CROSS_CHK_UP ;
call SPIN_FREE7 ; 50% power for the start
call WDT_OFF
WAIT_T2_OVER34: jnb TF2H, WAIT_T2_OVER34
mov ON_DUTY, #0
jmp ZERO_CROSS5
;----------------
ZERO_CROSS3: call STATE3 ; U=1, V=3ST, W=0 -> U=3ST, V=1, W=0
; P0.2 and P0.3(V), P0.4 and P0.5(W) are connected togather by solder.
mov CPT0MX, #01000000b ; CP in-: P1.1(HALFU), P0.0(U) with DOWN slope
call INDUCKTIVE_KICK_DET_LH ;
call ZERO_CROSS_CHK_DOWN;
;call SPIN_FREE7 ; TEST ONLY 50% power run TEST
call ZERO_CROSS_OFF_DUTY
ZERO_CROSS4: call STATE4 ; U=3ST, V=1, W=0 -> U=0, V=1, W=3ST
mov CPT0MX, #01010010b ; CP in-: P1.3(HALFW), CP in+: P0.4(W) with UP slope
call INDUCKTIVE_KICK_DET_HL ; Inductive kick L->H and H->L detector
call ZERO_CROSS_CHK_UP ; Inputs are reversed.
;call SPIN_FREE7 ; TEST ONLY
call ZERO_CROSS_OFF_DUTY
ZERO_CROSS5: call STATE5 ; U=0, V=1, W=3ST -> U=0, V=3ST, W=1
mov CPT0MX, #00010101b ; , CP in-: P0.3(V), CP in+: P1.2(HALFV) with DOWN slope
call INDUCKTIVE_KICK_DET_HL ; Inductive kick L->H and H->L detector
call ZERO_CROSS_CHK_UP ;
;call SPIN_FREE7 ; TEST ONLY 50% power run TEST
call ZERO_CROSS_OFF_DUTY
;-----------------
ZERO_CROSS6: call STATE6 ; U=0, V=3ST, W=1 -> U=3ST, V=0, W=1
mov CPT0MX, #01000000b ; CP in-: P1.1(HALFU), CP in+: P0.0(U) with UP slope
call INDUCKTIVE_KICK_DET_HL ; Inductive kick L->L and H->L detector
call ZERO_CROSS_CHK_UP ;
;call SPIN_FREE7 ; TEST ONLY 50% power run TEST
call ZERO_CROSS_OFF_DUTY
ZERO_CROSS1: call STATE1 ; U=3ST, V=0, W=1 -> U=1, V=0, W=3ST
mov CPT0MX, #01010010b ; CP in-: P1.3(HALFW), CP in+: P0.4(W) with DOWN slope
call INDUCKTIVE_KICK_DET_LH ; Inputs are reversed. Inductive kick H->L to L->H detector
call ZERO_CROSS_CHK_DOWN;
;call SPIN_FREE7 ; TEST ONLY 50% power run TEST
call ZERO_CROSS_OFF_DUTY
ZERO_CROSS2: call STATE2 ; U=1, V=0, W=3ST -> U=1, V=3ST, W=0
mov CPT0MX, #00010101b ; CP in-: P0.3(V), CP in+: P1.2(HALFV)with UP slope
call INDUCKTIVE_KICK_DET_LH ; Inputs are reversed. Inductive kick H->L and L->H detector
call ZERO_CROSS_CHK_DOWN
;call SPIN_FREE7 ; TEST ONLY 50% power run TEST
call ZERO_CROSS_OFF_DUTY
;-----------------------
ZERO_CROSS_RUN_PWR_CONTROL:
;call SPIN_FREE ; TEST ONLY
THROTTLE_OFF_CHK5: mov TEMP3, CONTROL_DATA ; Get Rx input pulse to check stop
mov a, TEMP3 ; Get Rx input pulse
add a, #STICK_BACK_HYS ; hysteresis margin for Rx input pulse jitter
clr c
subb a, BEMF_FULL_ON ; MAX throttle position as 50% run in BEMF mode
jc RX_NOISE_CANCELLER
mov RX_NOISE_FILTER_COUNTER, #0
PWR_CNTRL5: ;mov a, NEED_MORE_T_ADVANCE_FLAG
;jz ZC_PWR_SETTING
mov a, SPIN_COUNTER
jnz ZC_PWR_SETTING
;mov NEED_MORE_T_ADVANCE_FLAG, #0
jmp ZERO_CROSS3_ ; 255 spin with 37% timing advance to avoid sync. out
ZC_PWR_SETTING: mov a, TEMP3
clr c
subb a, BEMF_FULL_ON ; MAX throttle position as 50% run in BEMF mode
; jc ZERO_CROSS3_ ; BUGBUG MUST cosider the stick hysteresis value.
jnc INC_DEC5
jmp ZERO_CROSS3_
INC_DEC5: subb a, ON_DUTY
;jmp ZERO_CROSS3_ ; 50% power TEST ONLY
jz ZERO_CROSS3_
jnc INC_PWR5
jmp DEC_PWR5
;-----------------
INC_PWR5: mov a, TMR3CN ; T3 is used for slow increase power-up timer as same as BEMF T2 mode
anl a, #01000000b ; Ckeck T3 low byte overflow flag
jnz INC_DUTY_5 ; T3 overflow?
jmp SET_ON_DUTY_TIMER ; Not yet
INC_DUTY_5: anl TMR3CN, #00111111b ; Clear T3 overflow flags
inc TEMP0 ; Also, TEMP0 is used for 256 loops slow power-up timer counter with T3
cjne TEMP0, #ZC_SAMPLING_DUTY_INC_TIMING, ZERO_CROSS3_
mov TEMP0, #0
inc ON_DUTY
jmp ZERO_CROSS3 ; Still in zero cross run <<<<<<<<
ZERO_CROSS3_: jmp ZERO_CROSS3 ; Still in zero cross run
DEC_PWR5: dec ON_DUTY
jmp ZERO_CROSS3 ; Still in zero cross run
;------------------
SET_ON_DUTY_TIMER: clr TR2 ; Stop T2
mov a, T2_ON_DUTY_LOW ; Get previous timing data
cpl a
mov TMR2L, a ; set T2 low byte data
mov a, T2_ON_DUTY_HIGH ; Get previous timing data
cpl a
mov TMR2H, a ; set T2 high byte data
START_DUTY_ON: clr TF2H ; clear T2 high byte over flow bit
setb TR2 ; start T2 count-up for power on duty time
jmp ZERO_CROSS3 ; Still in zero cross run
;ret
;------------------
GET_BEMF_02: jmp GET_BEMF1 ; long branch help
;---------------
RX_NOISE_CANCELLER: clr c
mov a, RX_NOISE_FILTER_COUNTER
add a, #1
clr c
mov RX_NOISE_FILTER_COUNTER, a
subb a, #Rx_Noise_Count ; almost 0.1 sec margin to protect Rx stick down noise
jnc GET_BEMF_02 ; get back to BEMF mode
jmp ZERO_CROSS3 ; Still in zero cross run
;------------
INDUCKTIVE_KICK_DET_LH: call START_T2
call WAIT_1uS ; comparator res. to avoid inductive kick front edge
;
IND_KICK_NEG_LEVEL_CHK:
mov a, CPT0CN ; Get Comparator result
anl a, #01000000b ; Get CP0OUT bit
jz IND_KICK_POS_EDGE_CHK ; The inductive kick is already done in off duty timing.
clr TR2
mov TMR2M_H, TMR2H
mov TMR2M_L, TMR2L
setb TR2
ret
IND_KICK_POS_EDGE_CHK: clr c
mov a, TMR2H
subb a, #IND_KICK_TIME_CHECK ; Sync out value in T2. Over 350uS to get zero coross point as over load.
jnc SMALL_ZC_MARGIN_POS
ZC_POS_CHK: call WAIT_1uS ; Comparator responce
mov a, CPT0CN ; Get Comparator result
anl a, #01000000b ; Get CP0OUT bit
jz IND_KICK_POS_EDGE_CHK ; checking the positive edge of inductive kick.
clr TR2
mov TMR2M_H, TMR2H
mov TMR2M_L, TMR2L
setb TR2
ret
SMALL_ZC_MARGIN_POS: call SPIN_FREE
mov a, ON_DUTY ; May be accelating. Power down to avoid un-sync.
clr c
subb a, #1
jc ZERO_ON_DUTY_POS
mov ON_DUTY, a
ZERO_ON_DUTY_POS: jmp ZC_POS_CHK
;---------------
COULD_NOT_CATCH_CROSS_POINT_: call SPIN_FREE
jmp MOTOR_UNUSUAL_STOP
;---------------
ZERO_CROSS_CHK_DOWN: call WAIT_10uS ; Avoid chatter
clr EA
ZERO_CROSS_CHK_D: mov a, TMR2H
clr c
subb a, #REAR_EDGE_TO_ZC_POINT ; Sync out value in T2. Over 500uS to get zero coross point as over load.
jnc COULD_NOT_CATCH_CROSS_POINT_ ; System shut down to protect FETs.
call WAIT_1uS ; comparator responce.
mov a, CPT0CN ; Get Comparator result
anl a, #01000000b ; Get CP0OUT bit
jnz ZERO_CROSS_CHK_D ; checking the DOWN SLOPE until over zero cross point
STOP_T2: clr TR2 ; stop the T2 count-up
setb EA
OVER_ZERO_CHECK:
; call SPIN_FREE ; TEST ONLY!!!!!!
;check_point: nop
OVER_ZERO_CHECK_H: mov a, TMR2H ; Checking the T2 is zero
jnz TIMING_ADVANCE ; No, It is normal
OVER_ZERO_CHECK_L: mov a, TMR2L
anl a, #11100000b ; Cut T2 low 4bits as Is the zero cross time less than 31 as 15uS?
jnz TIMING_ADVANCE ; No, It is normal
SET_TF2H: call SPIN_FREE
setb TF2H ; some thing wrong. speed up next T2 end check routine(T2 over flow check).
ret
;***************
TIMING_ADVANCE: mov R1, TMR2L
mov R2, TMR2H
call TIMING_ADVANCE_CALC
jmp T_ADV_ADJ_END
;
TIMING_ADVANCE_CALC: mov a, R2
clr c
rrc a ; 1/2 as 25% timing advance
mov R2, a
mov a, R1
rrc a
mov R1, a
;
;mov a, NEED_MORE_T_ADVANCE_FLAG
;jnz TWO_BIT_RIGHT_SHIFT
ret
TWO_BIT_RIGHT_SHIFT: mov b, R1
mov TEMP_MEMORY, R2
mov a, R2
clr c
rrc a ; 1/4 as 1/8 12.5% Timing advance(1/2 phase measuring)
mov R2, a
mov a, R1
rrc a
mov R1, a
;
;ret
ADD_FOR_37_PERCENT_AD_TIMING: ; 37.5% advance for only start of zero cross control
clr c
add a, b
mov R1, a
mov a, R2
addc a,TEMP_MEMORY
mov R2, a
ret
;
mov a, R2
clr c
rrc a ; 1/8 as 1/16 6.3% Timing advance(1/2 phase measuring)
mov R2, a
mov a, R1
rrc a
mov R1, a
ret
;
mov a, R2
clr c
rrc a ; 1/16 as 1/32 3% Timing advance(1/2 phase measuring)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -