Schrittmotor mit pic16f84a

Hallo zusammen,
Bin neu hier und hab auch schon gleich ma ne Frage.
Ich hatte vor mir den oben genannten PIC für die Steuerung eines
Schritmotors zu programmieren und zwar ist auf der rotierenden Achse des
Motors ein runds Blatt befestigt mit einem loch andem die Lichtschranke
beim vorbeikommen ein High Pegel zeigt.
Unter den rotierenden Blatt ist ein befestigtes Blatt mit Zaheln von 0-9
in gleichmäsigem abstand 360° verteilt.
wenn der pic startet soll er sich unabhängig von seiner Position erst
einmal orientieren (mit Interrupt gelöst) was auch funktioniert (er
orientiert sich an Position 0 also die Lichtschranke). Nun soll der
Motor wenn man Hexadezimal eine Zahl an den Ports eingibt und diese
durch drücken des übername Taster dann auch eingelesen wird (High beim
betätigen) (vorher Binär gewandelt funktioniert auch einbandfrei) auch
auf diese Zahl zeigen (Es Wird Hex nur 0-9 benutz(Logisch)). Dort liegt
der Haken wenn ich die zweite Programm hälfte also das nachdem Interrupt
in dem Pic auch noch einfüge Kallibriert er sich zwar mit Linksdrehung
aber schwenkt dann nach rechts um und dreht sich nurnoch wie verrückt.

Ich wies das es sich nach viel anhört hoffe allerdings dennoch das ihr
mir helfen könnt da ich das fertige Programm dringent morgen brauche!
hier noch die benutzten Ports:
B Für das Bitmuster des Motors:
RA0= 2^0
RA1= 2^1
RA2= 2^2
RA3= 2^3

Übernahme Taster der beim drücken High Pegel gibt:
RA4

Lischtschrancke (Position0)
RB0

Standorteingabe also die hexadezimale eingabe:
RB1=2^0
RB2=2^1
RB3=2^2
RB4=2^3


Und hier noch mein Programm was nicht funktioniert:
list p=16f84
include <P16F84.inc>
ZAHL05 equ 0x0C
ZAHL5 equ 0x0D
KONST equ 0x0E
KONSTISR equ 0x0F
W_TEMP equ 0x10
STATUS_TEMP equ 0x11
TEMP_EIN equ 0x12
TEMP_AUS equ 0x13
TEMP equ 0x14
ZAHL equ 0x15

org 0x0000
goto INIT

org 0x0004
PUSH
movwf W_TEMP
swapf STATUS,0
movwf STATUS_TEMP
ISR
movlw 0x01
movwf KONSTISR
POP
swapf STATUS_TEMP,0
movwf STATUS
swapf W_TEMP,1
swapf W_TEMP,0

bcf INTCON,INTF
retfie

INIT
bsf STATUS,RP0
movlw 0x10
movwf TRISA
movlw 0xFF
movwf TRISB
bsf OPTION_REG,INTEDG
bcf STATUS,RP0
bsf INTCON,INTE
bsf INTCON,GIE

HP
movlw 0x02
movwf KONSTISR
movlw 0x04
movwf KONST
WIEDL
movf KONST,0
call TABLI
movwf PORTA
call ZEIT5
decfsz KONST,1
goto WIEDL
decfsz KONSTISR,1
goto HP
bcf INTCON,GIE
clrf TEMP_AUS

WIED0
btfss PORTB,0
goto WIED0
movf PORTB,0
andlw 0x1E
movwf TEMP_EIN
rrf TEMP_EIN,1
movlw 0x05
subwf TEMP_EIN,0
movwf TEMP
movf TEMP_EIN,0
movwf TEMP_AUS
btfss STATUS,C
goto POS1
goto POS2
POS1
call LINKS
goto WIED
POS2
call RECHTS

WIED
btfss PORTA,4
goto WIED
movf PORTB,0
andlw 0x1E
movwf TEMP_EIN
rrf TEMP_EIN,1
movf TEMP_AUS,0
subwf TEMP_EIN,0
movwf TEMP
movf TEMP_EIN,0
movwf TEMP_AUS
btfss STATUS,C
goto POS9
goto POS10
POS9
call LINKS
goto WIED
POS10
call RECHTS
goto WIED

ZEIT05
movlw 0x7D
movwf ZAHL05
WIED1
nop
decfsz ZAHL05,1
goto WIED1
return

ZEIT5
movlw 0x0A
movwf ZAHL5
nop
WIED2
call ZEIT05
decfsz ZAHL5,1
goto WIED2
return

TABRE
addwf PCL,1
nop
retlw 0x06
retlw 0x05
retlw 0x09
retlw 0x0A

TABLI
addwf PCL,1
nop
retlw 0x09
retlw 0x05
retlw 0x06
retlw 0x0A

LINKS
POS5
movlw 0x14
movwf ZAHL
POS3
movlw 0x04
movwf KONST
POS4
movf KONST,0
call TABLI
movwf PORTA
call ZEIT5
decfsz KONST,1
goto POS4
decfsz ZAHL,1
goto POS3
decfsz TEMP,1
goto POS5
return

RECHTS
POS6
movlw 0x14
movwf ZAHL
POS7
movlw 0x04
movwf KONST
POS8
movf KONST,0
call TABRE
movwf PORTA
call ZEIT5
decfsz KONST,1
goto POS8
decfsz ZAHL,1
goto POS7
decfsz TEMP,1
goto POS6
return

Ende
end

Ach und Arbeiten tue ich mit MPLAB.
Hoffe ihr könnt mir bald helfen das Program zum laufen zu bekommen!
Danke im Vorraus!!!