#include <P16F628A.INC>
__CONFIG _WDT_OFF
 cont0 EQU 0x20
 cont1 EQU 0x21
 cont2 EQU 0x22
 curva EQU 0x23
 posicao_inicial EQU 0x24
org 0x00
 goto setup

org 0x04
 retfie

setup
 banksel TRISB
 movlw b'11100000'
 movwf TRISB
 banksel PORTB
 movfw PORTB
 clrf PORTB
 banksel posicao_inicial
 movwf posicao_inicial
 movlw 0x07
 movwf curva
 goto main

main
 call reto
 btfsc PORTB, 5; bit 5 informa se está em curva
  goto trat_curva1 ;se estiver, chama a rotina
 goto main

reto
 banksel PORTB
 ;Configurando o MOTOR1
 bsf PORTB,RB0
 bcf PORTB,RB1
 ;Configurando o MOTOR2
 bsf PORTB,RB2
 bcf PORTB,RB3
 call atraso
 return

trat_curva1
 btfss curva, 2
  goto trat_curva2; vai pro btfss curva, 1
 btfsc posicao_inicial, 7
  call curva_direita
 btfss posicao_inicial, 7
  call curva_esquerda
 bcf curva, 2
 call atraso
 goto main

trat_curva2
 btfss curva, 1
  goto trat_curva3;vai pro btfss curva, 0
 btfsc posicao_inicial, 6
  call curva_direita
 btfss posicao_inicial, 6
  call curva_esquerda
 bcf curva, 1
 call atraso
 goto main

trat_curva3
 ;btfsc curva, 0
  ;goto setup2
 goto fim

setup2
 banksel posicao_inicial
 comf posicao_inicial, 1
 movlw 0x06
 movwf curva
 call cento_e_oitenta
 call atraso
 goto main

;<curvas>
curva_esquerda
 banksel PORTB
 bcf PORTB, 0
 bcf PORTB, 1
 return

curva_direita
 banksel PORTB
 bcf PORTB, 2
 bcf PORTB, 3
 return

cento_e_oitenta
 banksel PORTB
 bsf PORTB, 0
 bcf PORTB, 1
 bcf PORTB, 2
 bsf PORTB, 3
 return
;</curvas>


;<atraso>
atraso
 banksel cont0
 movlw 0xff
 movwf cont0
 goto atraso_00

atraso_00
 movlw 0x80
 movwf cont1
 decfsz cont0
  goto atraso_01
 return

atraso_01
 movlw 0x40
 movwf cont2
 decfsz cont1
  goto atraso_02
 goto atraso_00

atraso_02
 decfsz cont2
  nop
 goto atraso_01
;</atraso>

fim
 banksel PORTB
 clrf PORTB
 goto fim
end