Ping Roam V1.1.2

# Ping Object Avoidance
# Using Ultra Sonic Ping Sensor on servo for avoidance
# Version 1.1.2


# Configuration Settings
# This script assumes the following ports;
#
# Trig Port = D0
# Echo Port = D1
# Sweep Servo = D2

# Adjust values below for movement control
$reverseturn = 1 # Reverse before turn? 0 = no, 1 = yes
$maxdistance = 30 # Change for maximum distance from object before avoiding in units
$boxedindistance = 20 # Change for maximum distance for boxed in detection
$turnamount = 500 # Change for how long to turn for in ms
$reverseamount = 500 # Change for how long to reverse for in ms (if applicable)
$movementspeed = 255 # Change for movement speed
$slowturn = 127 # Change for slow turn speed

# Adjust values below for sweep configuration
$sweepmin = 10 # Change for min limit
$sweepmax = 90 # Change for max limit
$sweepservodelay = 500 # Change for delay between sweep servo movements and readings

# Testing Options
$testmode = 0 # Change to 1 if testing without PING sensor



# Do not adjust these values
# Calculate the centre position for the sweeping servo
# Minimum position plus maximum position divided by 2
# Round to 0 decimal places
$sweepcenter = Round((($sweepmin+$sweepmax)/2),0)
# Set the previous position to the max position for initial movement
$sweepprevious = $sweepmax # Do not change
# Set the current position to the centre position for initial movement
$sweepcurrent = $sweepcenter # Do not change
# Set some flags for testing - to be removed in final release
$SweepErrorFlag = 0 # Do not change
$BoxedInErrorFlag = 0 # Do not change
$BoxedInRun = 0 # Do not change
$SweepCenterRun = 0 # Do not change
$EscapeRun = 0 # Do not change
$ScriptErrorFlag = 0 # Do not change
$isboxedin = 0 # Do not change
# Set last and penultimate moves.
$penultimatemove = "none"
$lastmove = "none"



# ------- The Script --------

# Set the start point for any loops
:begin

# Center the sweep servo
Servo(D2, $sweepcenter)

# Start moving forwards
FORWARD()

# Start the detection
Goto(detect) # This line is redundant but left for educational purposes



# Detection code

# Set a label for loops and gotos
:detect

# Use random numbers if in test mode - to be removed in final release
IF ($testmode=0)
  $currentdistance = GetPing(D0, D1)
ELSE
  $currentdistance = GetRandom(0,255)
ENDIF

  # Check the current distance against the max allowed distance
IF ($currentdistance <= $maxdistance)
  # If the current distance is below the max distance start avoiding
  GOTO(avoid)

  # Set label for avoid return to avoid return errors
  :avoidreturn
ENDIF

# Run the sweeping servo code
GOTO(sweep)

# Wait
SLEEP ($sweepservodelay)

# Loop back to the start of detection
GOTO(detect)



# Avoidance code

# Set a label for loops and gotos
:avoid

# First check if boxed in
Goto(boxedin)

# If the robot is boxed in run the escape code
IF ($isboxedin = 1)
  Goto(escape)

  # Avoid return error after escape loop by setting a label for a goto
  :escapereturn

# Change to else to avoid reversing and turning after an escape - dont forget the endif at the bottom
ELSE

  # Check to see if to reverse before turning
  IF ($reverseturn = 1)
    # If the option of reverse before turning is set stop then reverse
    # Add in a stop and sleep before movement
    Stop()
    Sleep(200)
    Reverse($movementspeed,$reverseamount)
  ENDIF

  # Check the servo position

  # Check if it's to the left
  IF ($sweepcurrent = $sweepmin)
    # If the servo is in the lowst position (left) move right
    Goto(moveright)
    # Continue moving forwards
    # Add in a stop and sleep before movement
    Stop()
    Sleep(200)
    FORWARD()

  # Else check if it's to the right
  ELSEIF ($sweepcurrent = $sweepmax)
    # If the servo is in the highest position (right) move left
    Goto(moveleft)
    # Continue moving forwards
    # Add in a stop and sleep before movement
    Stop()
    Sleep(200)
    FORWARD()

  # Else assume it's in the middle
  ELSE
    # If the servo is in the center position check which side is closest to the object and move the other way

    # Move and check the left side
    Servo(D2,$sweepmin)
    # Use random numbers if in test mode
    IF ($testmode=0)
      $pingmin = GetPing(D0, D1)
    ELSE
      $pingmin = GetRandom(0,255)
    ENDIF

    # Wait
    Sleep(400)

    # Move and check the right side
    Servo(D2,$sweepmax)
    # Use random numbers if in test mode
    IF ($testmode=0)
      $pingmax = GetPing(D0, D1)
    ELSE
      $pingmax = GetRandom(0,255)
    ENDIF

    # Wait
    Sleep(400)

    # Move and check the center
    Servo(D2,$sweepcenter)
    IF ($pingmin > $pingmax)
      Goto(moveright)
      # Add in a stop and sleep before movement
      Stop()
      Sleep(200)
      FORWARD()
    ELSE
      Goto(moveleft)
      # Add in a stop and sleep before movement
      Stop()
      Sleep(200)
      FORWARD()
    ENDIF
  ENDIF
ENDIF

# Return to the main code
Goto(avoidreturn)



# The sweep code

# Set a label for loops and gotos
:sweep

# Move in the correct direction and store previous position

# Check what the current position is
# Check if left
IF ($sweepcurrent = $sweepmin)

  # Save the current position as the previous
  $sweepprevious = $sweepcurrent

  # Move to the next position
  Servo(D2, $sweepcenter)

  # Save the current position
  $sweepcurrent = GetServo(D2)

# Else check if its center and where it was before
# If it is center and was left before
ELSEIF ($sweepcurrent = $sweepcenter and $sweepprevious = $sweepmin)

  # Save the current position as the previous
  $sweepprevious = $sweepcurrent

  # Move to the next position
  Servo(D2, $sweepmax)

  # Save the current position
  $sweepcurrent = GetServo(D2)

# If it is center and was right before
ELSEIF ($sweepcurrent = $sweepcenter and $sweepprevious = $sweepmax)

  # Save the current position as the previous
  $sweepprevious = $sweepcurrent

  # Move to the next position
  Servo(D2, $sweepmin)

  # Save the current position
  $sweepcurrent = GetServo(D2)

# Else check if right
ELSEIF ($sweepcurrent = $sweepmax)

  # Save the current position as the previous
  $sweepprevious = $sweepcurrent

  # Move to the next position
  Servo(D2, $sweepcenter)

  # Save the current position
  $sweepcurrent = GetServo(D2)

# Else something has gone wrong
ELSE
  # Set an error flag for debugging purposes - to be removed for final release
  $SweepErrorFlag = 1
ENDIF

# Return back to the main script
Return()



# The sweep center code

# Set a label for loops and gotos
:sweepcenter

# Set a flag so we know it has run for debugging purposes
$SweepCenterRun = 1

# Move the servo to the left position
Servo(D2,$sweepmin)

# Use random numbers if in test mode
IF ($testmode=0)
  $pingmin = GetPing(D0, D1)
ELSE
  $pingmin = GetRandom(0,255)
ENDIF

# Wait
Sleep(200)

# Move the servo to the right
Servo(D2,$sweepmax)

# Use random numbers if in test mode
IF ($testmode=0)
  $pingmax = GetPing(D0, D1)
ELSE
  $pingmax = GetRandom(0,255)
ENDIF

# Wait
Sleep(200)

# Move the servo back to the center
Servo(D2,$sweepcenter)

# Check which side has the closest object
# If the object to the left is further away than the object to the right
IF ($pingmin > $pingmax)

  # Move to the right
  RIGHT($movementspeed,$turnamount)

  # Move forwards again
  # Add in a stop and sleep before movement
  Stop()
  Sleep(200)
  FORWARD()

# Else if the object to the right is further away than the object to the left
ELSEIF ($pingmin < $pingmax)

  # Move to the left
  LEFT($movementspeed,$turnamount)

  # Move forwards again
  # Add in a stop and sleep before movement
  Stop()
  Sleep(200)
  FORWARD()

# Else they are both the same
ELSE

  # So move left - this can be customised
  LEFT($movementspeed,$turnamount)

  # And move forwards again
  # Add in a stop and sleep before movement
  Stop()
  Sleep(200)
  FORWARD()
ENDIF

# Return to the main code
Return()



# The boxed in code

# Set a label for loops and gotos
:boxedin

# Set a flag so we know it has run for debugging - to be removed for final release
$BoxedInRun = 1

# Get distance to the side
# Move the servo to the left
Servo(D2,$sweepmin)

# Use random numbers if in test mode - to be removed for final release
IF ($testmode=0)
  $side1scan = GetPing(D0, D1)
ELSE
  $side1scan = GetRandom(0,255)
ENDIF

# Get distance to the other side
# Move the servo to the right
Servo(D2,$sweepmax)

# Use random numbers if in test mode
IF ($testmode=0)
  $side2scan = GetPing(D0, D1)
ELSE
  $side2scan = GetRandom(0,255)
ENDIF

# Get distance to the front
# Move the servo to the center
Servo(D2,$sweepcenter)

# Use random numbers if in test mode
IF ($testmode=0)
  $centerscan = GetPing(D0, D1)
ELSE
  $centerscan = GetRandom(0,255)
ENDIF

# Check if boxed in by compairing the results against a fixed boxed in distance
IF ($side1scan < $boxedindistance and $side2scan < $boxedindistance and $centerscan < $boxedindistance)

  # If any are true set the boxed in flag
  $isboxedin = 1

ENDIF

# Return to the main script
Return()



# The escape code

# Set a label for loops and gotos
:escape

# Set a flag so we know it has run for debugging
$EscapeRun = 1

# Reset the boxed in flag
$isboxedin = 0

# Center the sweep servo
Servo(D2,$sweepcenter)

# Turn slowly
Left($slowturn)

# Set up a loop
:escapeloop

# Scan until clear
# Use random numbers if in test mode
IF ($testmode=0)
  $escapescan = GetPing(D0, D1)
ELSE
  $escapescan = GetRandom(0,255)
ENDIF

# If the scan result is below the boxed in distance loop
IF ($escapescan < $BoxedInDistance)

  # Go back to the start of the escape loop
  Goto(escapeloop)

ENDIF

# Continue forwards
# Add in a stop and sleep before movement
Stop()
Sleep(200)
FORWARD()

# Return to the main script
Goto(escapereturn)



# Move Right code

# Set a label for loops and gotos
:moveright

# Check the last 2 moves to avoid left right left right loops
IF ($lastmove = "left" and $penultimatemove = "right")

  # If it has been right then left dont move right again but escape from a loop
  Goto(escape)

  # Reset the last move
  $lastmove = "none"

  # Else just move right
ELSE
  RIGHT($movementspeed,$turnamount)
  Sleep(200)

  # Save the penultimate move
  $penultimatemove = $lastmove

  # Save the last move
  $lastmove = "right"

ENDIF

# Go back to the main script
Return()



# Move left code

# Set a label for loops and gotos
:moveleft

# Check the last 2 moves to avoid left right left right loops
IF ($lastmove = "right" and $penultimatemove = "left")

  # If it has been left then right dont move left afain but escape from a loop
  Goto(escape)

  # Reset the last move
  $lastmove = "none"

  # Else just move left
ELSE
  LEFT($movementspeed,$turnamount)
  Sleep(200)

  # Save the penultimate move
  $penultimatemove = $lastmove

  # Save the last move
  $lastmove= "left"

ENDIF

# Go back to the main script
Return()

# End of scripts