MPLAB kod

Hej! Vi ska programmera en robotbil. Basic funktion är att när någon av sensorerna aktiveras ska den backa och svänga för att sen fortsätta köra, jag vill lägga till så att hur mycket den svänger beror på hur länge den åker framåt. Ni behöver bara kommentera hur ifall koden skulle möjliggöra att svänga beroende på räknaren, men inte hur mycket minne jag ska ladda varje cell i, det räknar jag ut senare. Har då skrivit följande stycke kod dock finns det mer som måste inkluderas i sekvensen, men den behöver jag inte ha med här:
A0 och A1 är sensorerna fram på bilen.
B6, B7, B4 och B5 är motorerna ser ut såhär och hur de för bilen framåt, upp är fram. Sedan har vi B3 och B2 som är LEDs som jag inte använder än.

/\ /\
l l
B6 B4

B7 B5
l l
\/ \/

bsf STATUS,RP0 ;öppnar bank 1 för programmering av I/O
movwf 0xff
movwf TRISB
movlw 0xff
movwf TRISA
bcf STATUS,RP0 ;stänger bank 1 för programmering av I/O
movwf ram1
movlw 0xff
movwf ram2
movlw 0xff
movwf ram3
movlw 0xff
goto Main

Main
bsf PORTB,4
bsf PORTB,6
bcf PORTB,2
bcf PORTB,3
bcf PORTB,5
bcf PORTB,7
goto DelayTurn

DelayTurn
decfsz ram1,f
btfsc PORTA,0
goto BackH
btfsc PORTA,1
goto BackV
goto DelayTurn
decfsz ram2,f
btfsc PORTA,0
goto BackH
btfsc PORTA,1
goto BackV
goto DelayTurn
decfsz ram3,f
btfsc PORTA,0
goto BackH
btfsc PORTA,1
goto BackV
goto DelayTurn
goto DelayTurn

DelayLoop
decfsz ram1,f
goto DelayLoop
decfsz ram2,f
goto DelayLoop
decfsz ram3,f
goto DelayLoop
return

BackH
bsf PORTB,7
bsf PORTB,5
call DelayLoop
bcf PORTB,7
bsf PORTB,6
call DelayTurn
bcf PORTB,6
bcf PORTB,5
goto Main

BackV
bsf PORTB,7
bsf PORTB,5
call DelayLoop
bcf PORTB,5
bsf PORTB,4
call DelayTurn
bcf PORTB,4
bcf PORTB,7
goto Main
end
-----------------------------------------------
Ifall ni har några funderingar kring något är det bara att fråga!