'------------------------------------
' Lawn mower robot controll code
'  Dan Williams 2001
' Ver 1.6
'------------------------------------

verbose = 1
GOSUB ginit   ' initialize system

GOSUB forward ' set initial conditions

there:        ' main loup

'------------------ WANDER --------------

 IF ((verbose > 0) AND (dbg$ <> "")) THEN
     PRINT (dbg$)
     dbg$ = ""
 END IF

 timeTrigger = (-1)
 PRINT "ok -> "; ok
 IF (ok <> 0) THEN
  timeTrigger = TIMER + ok
  ok = 0
 END IF

 event = 0
 WHILE (event = 0)

  key$ = INKEY$
  time = TIMER
  GOSUB getfeelers

  IF (key$ <> "") THEN GOSUB handleKey
  IF (obumperstat <> bumperstat) THEN event = 1
  IF (timeTrigger <> (-1)) THEN
   IF (time > timeTrigger) THEN
      PRINT "time alarm"
      event = 1
      timeTrigger = (-1)
   END IF
  END IF

 WEND
 PRINT "event!"
 obumperstat = bumperstat
 lastaction$ = action$

 IF (0) THEN
 ELSEIF (fbumper OR (lwisker AND rwisker)) THEN    ' if we hit something dead on
   action$ = "front"
   GOSUB handleFront
 ELSEIF (lwisker) THEN                             ' if we sense a sidewall
   action$ = "left"
   GOSUB handleLeft
 ELSEIF (rwisker) THEN                             ' if we sense a sidewall
   action$ = "right"
   GOSUB handleRight
 ELSE                                              'otherwise
  IF (ok <> 0) THEN
     IF (verbose) THEN dbg$ = "timing last operation"
  ELSE
    IF (verbose) THEN dbg$ = "resuming forward"
    action$ = "resume"
    GOSUB forward
  END IF
 END IF

 GOTO there
 END

'////////////////////////////////////////////////
' ------------- handler routines ---------------
'\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\


'------------------- keyboard command -----------------
handleKey:

  IF (key$ = "r") THEN
    maskfeelers = 0
  ELSE
    maskfeelers = 1
  END IF

  IF (0) THEN
  ELSEIF (key$ = "5") THEN
    GOSUB halt
  ELSEIF (key$ = "r") THEN
    GOSUB forward
  ELSEIF (key$ = "1") THEN
    GOSUB backleft
  ELSEIF (key$ = "2") THEN
    GOSUB reverse
  ELSEIF (key$ = "3") THEN
    GOSUB backright
  ELSEIF (key$ = "6") THEN
    GOSUB spinright
  ELSEIF (key$ = "9") THEN
    GOSUB forwardright
  ELSEIF (key$ = "8") THEN
    GOSUB forward
  ELSEIF (key$ = "7") THEN
    GOSUB forwardleft
  ELSEIF (key$ = "4") THEN
    GOSUB spinleft
  END IF

RETURN


'------------------- front collision ----------------
handleFront:

  ok = 5
  GOSUB halt
  SOUND 400, 4

  IF (lwisker <> rwisker) THEN 'directional hint
    IF (lwisker) THEN 'turn left
      IF (verbose) THEN dbg$ = "front bumper; hinting left"
      GOSUB right
      dir = 1
    ELSE 'turn right
      IF (verbose) THEN dbg$ = "front bumper; hinting right"
      GOSUB left
      dir = 0
    END IF
  ELSE 'guess
    'IF (verbose) THEN dbg$ = "front bumper; guessing"
    IF (INT(RND(1) * 100) > 50) THEN
      IF (verbose) THEN dbg$ = "front bumper; guessing - left"
      'GOSUB left
      dir = 0
    ELSE
      IF (verbose) THEN dbg$ = "front bumper; guessing - right"
      'GOSUB right
      dir = 1
    END IF
  END IF

  GOSUB reverse
  GOSUB delay
  GOSUB halt
  GOSUB delay

  IF (dir = 0) THEN
    GOSUB right
  ELSE
    GOSUB left
  END IF

  GOSUB delay
  GOSUB halt

RETURN

'------------------ Left collision ---------------
handleLeft:

 IF (action$ <> lastaction$) THEN

  GOSUB halt
  IF (verbose) THEN dbg$ = "left wisker triggered"
  ok = 3
  IF (fwdir < 0) THEN
   GOSUB backright ' fix
  ELSE
   GOSUB backleft ' fix
  END IF

 ELSE ' this is our timer returning, as in WERE STUCK

  ok = 3 ' call us back in 3 secs
  IF (verbose) THEN dbg$ = "left wisker REtriggered"
  GOSUB forward
  GOSUB shortdelay

  IF (fwdir < 0) THEN
   GOSUB spinright ' fix
  ELSE
   GOSUB spinleft ' fix
  END IF

  GOSUB shortdelay

 END IF

RETURN



'---------------- Right collision ----------------
handleRight:

 IF (action$ <> lastaction$) THEN

  GOSUB halt
  IF (verbose) THEN dbg$ = "right wisker triggered"
  ok = 3
  IF (fwdir < 0) THEN
   GOSUB backleft 'fix
  ELSE
   GOSUB backright ' fix
  END IF

 ELSE

  ok = 3
  IF (verbose) THEN dbg$ = "right wisker REtriggered"
  GOSUB forward
  GOSUB shortdelay

  IF (fwdir < 0) THEN
   GOSUB spinleft 'fix
  ELSE
   GOSUB spinright ' fix
  END IF

  GOSUB shortdelay

 END IF

RETURN


'////////////////////////////////////////////////
' ------------------ io routines ---------------
'\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\

ginit:
  CLS
  rwisker = 0          ' initial sensor states
  lwisker = 0
  fbumper = 0
  rbumper = 0
  maskfeelers = 0

  IF (1) THEN
   OPEN "settings.rob" FOR INPUT AS #1

   INPUT #1, s$
   port = VAL(s$)
   PRINT "port: "; port

   INPUT #1, s$
   motorlf = VAL(s$)
   PRINT "motorlf: "; motorlf
   INPUT #1, s$
   motorlb = VAL(s$)
   PRINT "motorlb: "; motorlb
   INPUT #1, s$
   motorrf = VAL(s$)
   PRINT "motorrf: "; motorrf
   INPUT #1, s$
   motorrb = VAL(s$)
   PRINT "motorrb: "; motorrb

   INPUT #1, s$
   sensorr = VAL(s$)
   PRINT "sensorr: "; sensorr
   INPUT #1, s$
   sensorl = VAL(s$)
   PRINT "sensorl: "; sensorl
   INPUT #1, s$
   sensorf = VAL(s$)
   PRINT "sensorf: "; sensorf
   INPUT #1, s$
   sensorb = VAL(s$)
   PRINT "sensorb: "; sensorb

   INPUT #1, s$
   sensorinvertmask = VAL(s$)
   PRINT "sensorinvertmask: "; sensorinvertmask

   CLOSE #1
  END IF


  GOSUB halt       'stop!
  SOUND 1500, 2
  SOUND 1000, 1
  PRINT "Robot Loaded and ready to mo."
RETURN

getfeelers:

  orwisker = rwisker
  olwisker = lwisker
  ofbumper = fbumper
  orbumper = rbumper

  IF (maskfeelers) THEN
    bumperstat = 0
  ELSE
    bumperstat = (INP(port + 1) AND (8 + 16 + 128))
    bumperstat = bumperstat XOR sensorinvertmask
  END IF
  

  rwisker = (bumperstat AND sensorr) / sensorr
  lwisker = (bumperstat AND sensorl) / sensorl
  fbumper = (bumperstat AND sensorf) / sensorf
  rbumper = 0' (bumperstat AND sensorb) / sensorb

RETURN

halt:
  PRINT "halt"
  fwdir = 0
  OUT port, 0
RETURN

reverse:
  PRINT "reverse"
  fwdir = -1
  OUT port, (0 OR motorrb OR motorlb)
RETURN


forward:
  PRINT "forward"
  fwdir = 1
  OUT port, (0 OR motorrf OR motorlf)
RETURN

straighter:
  IF (fwdir = 1) THEN
    GOSUB forward
  ELSEIF (fwdir = (-1)) THEN
    GOSUB reverse
  ELSE
    GOSUB halt
  END IF
RETURN

forwardright:
  PRINT "forwardright"
  OUT port, (0 OR motorlf)
RETURN

spinright:
  PRINT "spinright"
  OUT port, (0 OR motorrb OR motorlf)
RETURN

right:
backright:
  PRINT "backright"
  OUT port, (0 OR motorrb)
RETURN

forwardleft:
 PRINT "forwardleft"
 OUT port, (0 OR motorrf)
RETURN

spinleft:
 PRINT "spinleft"
 OUT port, (0 OR motorrf OR motorlb)
RETURN

left:
backleft:
  PRINT "backleft"
  OUT port, (0 OR motorlb)
RETURN


shortdelay:
  FOR d = 1 TO 700
    IF (INKEY$ = "0") THEN
      GOSUB halt
      GOTO there
    END IF
  NEXT d
RETURN

delay:
  FOR d = 1 TO 2500
    IF (INKEY$ = "0") THEN
      GOSUB halt
      GOTO there
    END IF
  NEXT d
RETURN