Wall Following Examples (video and code)

Martin_H

Matt put out the call for some code samples to show off Parallax gear at the Expo. I'm planning on two wall following examples. The first uses a single Ping))) sensor with a BS2e controlling the CBA. It should run nearly unmodified on a Boe-bot once you change the stamp directive.

This sample follows a wall using arcs towards the wall and then a turn away from the wall. Like most of my good ideas it came from Robot Builders Bonanza

[video=youtube_share;xkYpWYhpIis]

Here's the code:

' ==================================================  ============================
'
'   File...... SimpleWallFollowing.BSE
'   Purpose... The robot follows a wall using a single ultrasonic sensor
'   Author.... Martin Heermance
'   E-mail.... mheermance@gmail.com
'   Started... 2012 March 31st
'   Updated... 2012 April st
'
'   {$STAMP BS2e}
'   {$PBASIC 2.5}
' ==================================================  ============================

' ------------------------------------------------------------------------------
' Program Description
' ------------------------------------------------------------------------------
' This program can be used with a CBA or Boe-bot to follow a wall using only a
' single ultrasonic sensor.  The basic concept used is from page 609 of "Robot
' Builder's Bonanza" by Gordon McComb

' ------------------------------------------------------------------------------
' Revision History
' ------------------------------------------------------------------------------

' Pin allocations.  You might need to tweak the Ping))) pin.
Ping           PIN  10  ' Pin for PING))) Sensor
RMotor         PIN  12  ' Pin for right servo
LMotor         PIN  13  ' Pin for left servo

' These values need tweaking for each robot's servos.
Speed075L      CON 831  ' Fast left
Speed075R      CON 658  ' Fast right
Speed050R      CON 685  ' Slow right (to arc)

' These values control distance from the wall and sharpness of left turn
WallDistance   CON   5
TurnCount      CON   40

' Standard values from Ping))) example
Trigger        CON    5 ' trigger pulse = 10 uS
Scale          CON $200 ' raw x 2.00 = uS
RawToIn        CON  889 ' 1 / 73.746 (with **)
RawToCm        CON 2257 ' 1 / 29.034 (with **)

' Trigger Ping)))
IsHigh         CON   1
IsLow          CON   0

' This program is so simple it only needs a single variable to
' compute and test the distance to the wall.
X              VAR Word

' Execution starts here
Main:
  ' Pause at start of program to let user set down robot.
  PAUSE 1000

  DO
    GOSUB PingDistance

    ' If we're far enough from the wall the continue the arc.
    IF (X > WallDistance) THEN
      ' Pulse once
      PULSOUT RMotor, Speed050R
      PULSOUT LMotor, Speed075L
      PAUSE 15
    ELSE
      ' Pulse turn count times to strongly turn away from the wall.
      FOR X = 1 TO TurnCount
        PULSOUT RMotor, Speed050R
        PULSOUT LMotor, Speed050R
        PAUSE 15
      NEXT
    ENDIF
  LOOP
  ' We'll never get here, but it's nice coding style.
  END

' PingDistance - reads the distance at the current head angle.
' Inputs:
' Outputs:
'   X - distance in inches.
PingDistance:
  Ping = IsLow                                ' make trigger 0-1-0
  PULSOUT Ping, Trigger                       ' activate sensor
  PULSIN  Ping, IsHigh, X                     ' measure echo pulse
  X = X */ Scale                              ' convert to uS
  X = X / 2                                   ' remove return trip
  X = X ** RawToIn                            ' convert to inches
  RETURN

Attached for easy downloading:

SimpleWallFollow.bse

Martin_H

This is the two sensor wall following example. It uses a front bumper coupled with a Ping))). The bumper prevents a collision with a wall in front while the robot looks sideways with the Ping))). It then uses that distance to determine if one or both wheels need to turn to maintain distance. When the front bumper triggers the robot turns to avoid the wall in front. Again the code is really simple and only requires a single variable.

[video=youtube_share;AQK6b2CNTrg]

Here's the code which is nearly identical to the previous sample except for a slightly altered main loop.

' ==================================================  ============================
'
'   File...... AdvancedWallFollow.BSE
'   Purpose... The robot follows a wall using an ultrasonic sensor and front bumper
'   Author.... Martin Heermance
'   E-mail.... mheermance@gmail.com
'   Started... 2012 April 2nd
'   Updated... 2012 April 3rd
'
'   {$STAMP BS2e}
'   {$PBASIC 2.5}
' ==================================================  ============================

' ------------------------------------------------------------------------------
' Program Description
' ------------------------------------------------------------------------------
' This program can be used with a CBA or Boe-bot to follow a wall using a Ping)))
' and a front bumper.  The Ping))) points to the right and keeps the robot away
' from the wall while the bumper prevents front collisions the basic concept used
' is from page 608 of "Robot Builder's Bonanza" by Gordon McComb

' ------------------------------------------------------------------------------
' Revision History
' ------------------------------------------------------------------------------

' Pin allocations.  You might need to tweak the Ping))) pin.
Bumper         PIN   9  ' Pin for front bumper (tactile or IR)
Ping           PIN  10  ' Pin for PING))) Sensor
RMotor         PIN  12  ' Pin for right servo
LMotor         PIN  13  ' Pin for left servo

' These values need tweaking for each robot's servos.
Speed075L      CON 831  ' Fast left
Speed075R      CON 658  ' Fast right
Speed050R      CON 685  ' Slow right (to arc)

' These values control distance from the wall and sharpness of left turn
MinWallDist    CON   6
MaxWallDist    CON   8
TurnCount      CON   40

' Standard values from Ping))) example
Trigger        CON    5 ' trigger pulse = 10 uS
Scale          CON $200 ' raw x 2.00 = uS
RawToIn        CON  889 ' 1 / 73.746 (with **)
RawToCm        CON 2257 ' 1 / 29.034 (with **)

' Trigger Ping)))
IsHigh         CON   1
IsLow          CON   0

' This program is so simple it only needs a single variable to
' compute and test the distance to the wall.
X              VAR Word

' Execution starts here
Main:
  ' Pause at start of program to let user set down robot.
  PAUSE 1000

  DO
    ' If we are not close to a wall in front
    IF (Bumper = 1)  THEN
      ' find out how close to a wall to our right.
      GOSUB PingDistance

      ' If we are too far from the wall pulse the left motor
      ' else if we are too close to the wall pulse only the right
      ' otherwise we are in the goldilocks zone and can do both
      IF (X > MaxWallDist) THEN
        PULSOUT LMotor, Speed075L
      ELSEIF (X < MinWallDist) THEN
        PULSOUT RMotor, Speed075R
      ELSE
        PULSOUT LMotor, Speed075L
        PULSOUT RMotor, Speed075R
      ENDIF
      PAUSE 15
    ELSE
      ' We're about to smack into a wall so turn sharply left!
      FOR X = 1 TO TurnCount
        PULSOUT RMotor, Speed050R
        PULSOUT LMotor, Speed050R
        PAUSE 15
      NEXT
    ENDIF
  LOOP

  ' We'll never get here, but it's nice coding style.
  END

' PingDistance - reads the distance at the current head angle.
' Inputs:
' Outputs:
'   X - distance in inches.
PingDistance:
  Ping = IsLow                                ' make trigger 0-1-0
  PULSOUT Ping, Trigger                       ' activate sensor
  PULSIN  Ping, IsHigh, X                     ' measure echo pulse
  X = X */ Scale                              ' convert to uS
  X = X / 2                                   ' remove return trip
  X = X ** RawToIn                            ' convert to inches
  RETURN

Attached for easy downloading:
AdvancedWallFollow.bse

Matt Gilliland

Sweet Martin! Will you be coming to Expo and bringing that little guy with you?
-MattG

Martin_H

Thanks Matt. Unfortunately I can't make it due to prior commitments and the long flight.

I published the videos and code to the forum in the hopes that someone who goes might use them and have some fun with them. They should run on a Boe-bot with just a Ping))) as no servo is used, one of the whiskers would work as the bumper.