Boebot random walk

random_walker.bs2

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

' 0 is forward mode (robot is moving)
' 1 is rotate mode (robot is rotating to randomAngle)
currentState VAR Bit

'Random bits for the random angle generation
randomBits VAR Word

'Random angle holder
randomAngle VAR Word

'Rotation time
rotationLoopCount VAR Word

'Current forward distance (time of flight)
currentDistance VAR Word

'Minimum forward distance allowed
minDistance VAR Word

servoPosCount VAR Word
servoPos VAR Word
pulseCount VAR Word
modCount VAR Word
freq VAR Word

Setup:
    currentState = 0
    minDistance = 500
    currentDistance = 0
    randomAngle = 0
    randomBits = 0
    modCount = 0
Main:
    GOSUB OrientRadarForward
    DO
      DEBUG "State: ", DEC currentState, CR
      IF (currentState = 0) THEN
        PAUSE 40
        GOSUB DistanceScan
        'DEBUG "Current dist: ", DEC currentDistance, CR
        DO WHILE (currentDistance > minDistance)
          GOSUB MoveForward
          IF ((modCount // 5) = 0) THEN
             GOSUB DistanceScan
          ENDIF
          DEBUG "d: ", DEC currentDistance, CR
          GOSUB PlaySound
          modCount = modCount + 1
        LOOP
        ' Switch to turn mode when an obstacle is close
        currentState = 1
      ELSE
        GOSUB Rotate
        ' Switch back to forward mode
        currentState = 0
      ENDIF
    LOOP
    DEBUG CLS, "Done"
    END ' end program

'Set the scan to aim forward.
OrientRadarForward:
  servoPos = 650
  PULSOUT 14, servoPos
  FOR pulseCount = 1 TO 10
    PULSOUT 14, servoPos
    PAUSE 40
  NEXT
  RETURN

MoveForward:
  PULSOUT 13, 815
  PULSOUT 12, 685
  PAUSE 20
  RETURN

' Read forward distance using sonar distance
DistanceScan:
  PAUSE 20
  PULSOUT 15, 5
  PULSIN 15, 1, currentDistance
  RETURN

'Rotate until angle is reached
Rotate:
  GOSUB GetRandomRotationAngle
  'Looping 84 times results in 360 deg rotation
  rotationLoopCount = ((162 + randomAngle) * 84) / 360
  'DEBUG "Rotation loops :", DEC rotationLoopCount, CR
  FOR pulseCount = 1 TO rotationLoopCount
      PULSOUT 13, 850
      PULSOUT 12, 850
      PAUSE 20
  NEXT
  RETURN

'Random small angle from
GetRandomRotationAngle:
  RANDOM randomBits   'stir up the bits of NumGen.
  randomAngle = randomBits / 1820 '0 TO 36 range.
  DEBUG "Rand angle: ", DEC randomAngle, CR
  RETURN

PlaySound:
  IF ((modCount // 20) = 0) THEN
    freq = ((currentDistance MIN 500) MAX 4500)
    freq = 5500 - freq
    FREQOUT 4, 100, freq
  ENDIF
  RETURN