Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Boebot random walk
' {$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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment