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

DFORW CON 15
DBACK CON 14
STEERR CON 2
STEERL CON 3


GOSUB init

DO
  GOSUB turnoff
  GOSUB driveoff
  PAUSE 2000

  GOSUB driveback
  PAUSE 1000
  GOSUB driveoff

  GOSUB skitchleft
  PAUSE 100


  GOSUB driveforward
  PAUSE 1000
  GOSUB turnright
  PAUSE 500
  GOSUB turnoff
  PAUSE 500
  GOSUB turnright
  PAUSE 600
  GOSUB turnoff
  PAUSE 500
  GOSUB turnright
  PAUSE 300
  GOSUB turnleft
  PAUSE 800
  GOSUB turnoff

  GOSUB turnoff
  GOSUB driveoff
  PAUSE 100

  GOSUB turnright
  GOSUB driveback
  PAUSE 1000

  GOSUB skitchleft
  PAUSE 500
  GOSUB skitchleft
  PAUSE 500
  GOSUB driveforward
  PAUSE 2000



  GOSUB driveoff
  GOSUB turnoff
  DEBUG "looped" , CR
LOOP

END


init:
  GOSUB turnoff
  GOSUB driveoff
RETURN

turnleft:

  LOW STEERR
  HIGH STEERL
RETURN

turnright:
  LOW STEERL
  HIGH STEERR
RETURN

turnoff:
  LOW STEERL
  LOW STEERR
RETURN


driveback:
  LOW DFORW
  HIGH DBACK
RETURN

driveforward:
  LOW DBACK
  HIGH DFORW
RETURN

driveoff:
  LOW DBACK
  LOW DFORW
RETURN

skitchleft:
  GOSUB turnleft
  GOSUB driveforward
  PAUSE 1000
  GOSUB turnoff
  GOSUB driveoff

  PAUSE 100

  GOSUB turnright
  GOSUB driveback
  PAUSE 1000
  GOSUB turnoff
  GOSUB driveoff
RETURN

skitchright:
  GOSUB turnright
  GOSUB driveforward
  PAUSE 1000
  GOSUB turnoff
  GOSUB driveoff

  PAUSE 100

  GOSUB turnleft
  GOSUB driveback
  PAUSE 1000
  GOSUB turnoff
  GOSUB driveoff
RETURN