pic16f877a проблема со сравнением 2-х чисел Опции

[Dmitriy_dda]

долго бьюсь над проблемкой:
есть 2 макроса сравнения 2-х чисел

; Snippet: 16-bit if(A >=
; Inputs: A_high, A_low, B_high, B_low
; Strategy: Do a 16-bit A-B and watch for borrow off 16th bit.
if_A_higher_B MACRO A_high, A_low, B_high, B_low, false, true
movf B_low, W
subwf A_low, W ; W = A-B (low)

movlw 0x00 ; Clear W
btfss STATUS, C ; Check for carry on low bytes.
movlw 0x01 ; C=0, B_low < A_low, needed to borrow (sub 1 from high)
; C=1, A_low > B_low, do not sub 1 from high byte

addwf B_high, W ; Adding 1 to B is like subtracting 1 from A (if W=1)
btfsc STATUS, C ; If B_high == FF, can borrow and not realize
goto false ; false cond'n
subwf A_high, W ; W = A-B (high) - borrow if applicable

btfss STATUS, C ; Check whole 16-bit addition for borrow
goto false ; C=0, result needs borrow from 17th bit.
; C=1, result true

; Place result TRUE code here.

goto true
ENDM

; Snippet: 16-bit if(A <=
; Inputs: A_high, A_low, B_high, B_low
; Strategy: Do a 16-bit B-A and watch for borrow off 16th bit.
if_A_lower_B MACRO A_high, A_low, B_high, B_low, false, true
movf A_low, W
subwf B_low, W ; W = B-A (low)

movlw 0x00 ; Clear W
btfss STATUS, C ; Check for carry on low bytes.
movlw 0x01 ; C=0, A_low < B_low, needed to borrow (sub 1 from high)
; C=1, B_low > A_low, do not sub 1 from high byte

addwf A_high, W ; Adding 1 to B is like subtracting 1 from A (if W=1)
btfsc STATUS, C ; If A_high == FF, can borrow and not realize
goto false ; false cond'n
subwf B_high, W ; W = B-A (high) - borrow if applicable

btfss STATUS, C ; Check whole 16-bit addition for borrow
goto false ; C=0, result needs borrow from 17th bit.
; C=1, result true
goto true
ENDM

и блок обработки в основном теле программы:

main:
clrwdt
pagesel CCP_drive
call CCP_drive ;действия по отсчетам ССР для АЦП и USART

;обращение к ШД:
;ждем некоторое время для регулирования скорости вращения
banksel M_Drive_speed_L
decfsz M_Drive_speed_L,f
goto main_1
movlw 0xFF
movwf M_Drive_speed_L
decfsz M_Drive_speed_H,f
goto main_1
mov_bb_to_bb R_Drive_speed_H,R_Drive_speed_L,M_Drive_speed_H,M_Drive_speed_L
pagesel Motor_drive
call Motor_drive

goto main ;цикл по новой

Motor_drive:
;M_Drive_nul EQU 0x23 ;указатель шага двигателя с виртуальным нулевым значением
;M_D_PTR_L EQU 0x24 ;текуший указатель шага на шаговом двигателе
;M_D_PTR_H EQU 0x25
;M_D_PTR_L_new EQU 0x26 ;требуемый указатель на шаговом двигателе
;M_D_PTR_H_new EQU 0x27

banksel M_D_PTR_H

if_A_higher_B M_D_PTR_H, M_D_PTR_L, M_D_PTR_H_new, M_D_PTR_L_new, M_d_start_inc, M_d_4
M_d_4:

if_A_lower_B M_D_PTR_H, M_D_PTR_L, M_D_PTR_H_new, M_D_PTR_L_new, M_d_start_dec, Motor_drive_end
M_d_start_dec:
;если (M_Drive_PTR > M_Drive_PTR_new) то
banksel M_D_PTR_L ;выбираем текуший указатель шага
movlw 0x01
subwf M_D_PTR_L,f ;уменьшить указатель текущего шага на 1
;movlw 0x00
btfsc STATUS,C
goto Motor_drive_result
banksel M_D_PTR_H
decf M_D_PTR_H,f ;если L был 0 стал 255 то уменшить H
goto Motor_drive_result
M_d_start_inc:
;если (M_Drive_PTR < M_Drive_PTR_new) то
banksel M_D_PTR_L ;выбираем текуший указатель шага
incfsz M_D_PTR_L,f ;увеличить указатель текущего шага на 1
goto Motor_drive_result
banksel M_D_PTR_H
incf M_D_PTR_H,f ;если L было 255 и стало 0 то inc H
Motor_drive_result:
;т.к. шаг 0=шагу_32=шагу_64=шагу_96 т.е. значимы для управления только первые 5 байт
banksel M_D_PTR_L
movf M_D_PTR_L,w ;для отсечения 6-х верхних байт - в аккумулятор
andlw b'00000011' ;получаем требуемое смещение
call Table_Motor_Drive_2 ;делаем требуемый шаг по вычесленному смещению
banksel PORTB
movwf PORTB ;посылаем в ПортB сигнал управления ШД
goto Motor_drive_end ;выход
Motor_drive_end:
return

Проблема в том, что даже при равных значениях M_D_PTR и M_D_PTR_new программа довольно часто вылетеает на прирашение или уменьшение текущего шага... Почему это происходит я не понимаю. Причем, если отключать все прерывания и АЦП и банально оставлять только подпрограмму "Motor_drive" то все впорядке.

Подскажите, в где может быть ошибка?