Line Following Smooth

' {$STAMP BS2}
' {$PBASIC 2.5}

sr VAR Bit
sl VAR Bit
fr VAR Bit
fl VAR Bit
n VAR Byte
bkc VAR Byte
cnt VAR Word
rspd VAR Word
lspd VAR Word
dir VAR Bit
dirf VAR Bit
RIGHT CON 0
LEFT CON 1
FWD CON 1
RWHEEL CON 12
LWHEEL CON 13

'dir = 0 is left
'dir = 1 is right
dir = Right
dirf = FWD
n = 0
bkc = 0
cnt = 0
'DEBUG CLS,
'      "frequency  detected", CR
DO
  'sl = IN4   'yellow
  fl = IN5   'blue
  fr = IN6   'white
  'sr = IN7   'green

  ' 0 is black
  ' 1 is white

  'PULSOUT 12 is right Wheel
  'PULSOUT 13 is left Wheel

  cnt = cnt + 1
  DEBUG CLS, DEC cnt, " is cnt", CR

  IF (fl = 1) OR (fr = 1) THEN
      HIGH 9
  ELSE
      LOW 9
  ENDIF
  IF (fl = 0) AND (fr = 0) THEN 'move forward
    dirf = FWD
    rspd = 650
    lspd = 850
    bkc = 0
   ' PAUSE 1
  ELSEIF (fl = 0) AND (fr = 1) THEN   'turn left
    dir = LEFT
    dirf = 0
    bkc = 1
    'GOSUB left
    rspd = 650
    lspd = 770
   ' PAUSE 1
  ELSEIF (fl = 1) AND (fr = 0) THEN    'turn right
    dir = RIGHT
    dirf = 0
    bkc = 1
    'GOSUB right
    rspd = 720
    lspd = 850
   ' PAUSE 1

 ELSEIF (fl = 1) AND (fr = 1) THEN           'backwards
    n = n + 2
    DEBUG DEC n, " is n"
    bkc = 1
    dirf = 0
    'GOSUB back

    IF (dir = RIGHT) THEN
      rspd = 725
      lspd = 840

    ELSEIF (dir = LEFT) THEN
      rspd = 660
      lspd = 755
    'PAUSE 250
    ELSE
      rspd = 850
      lspd = 650
    ENDIF
  ENDIF

  IF (cnt > 15) THEN
    n = 0
    cnt = 0
    DEBUG "i started cnt over"
    'PAUSE 250
  ENDIF

  IF (dirf = FWD)  THEN
    PULSOUT RWHEEl, rspd
    PULSOUT LWHEEL, lspd
    DEBUG CR, "i was gong forward"

  ELSEIF (bkc = 1) AND (dir = RIGHT) THEN
    PULSOUT RWHEEl, rspd + n
    PULSOUT LWHEEL, lspd
    DEBUG CR, "+n", DEC n, " = n"
    'PAUSE 1000

  ELSEIF (bkc = 1) AND (dir = LEFT) THEN
    PULSOUT RWHEEl, rspd
    PULSOUT LWHEEL, lspd - n
    DEBUG CR, "-n" , DEC n, " = n"
    'PAUSE 1000

  ENDIF

'PAUSE 1
LOOP