Ping Roam V1.1.0

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


# Configuration Settings

# Adjust values below for port configuration
$pingport = D0 # Change for Ping Port
$echoport = D1 # Change for Echo Port
$sweepservo = D2 # Change for sweep servo port

# 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
$sweepcenter = $sweepmin+$sweepmax
$sweepcenter = $sweepcenter/2
$sweepcenter = Round($sweepcenter,0)
$sweepprevious = $sweepmax # Do not change
$sweepcurrent = $sweepcenter # Do not change
$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
$penultimatemove = "none"
$lastmove = "none"



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

# Set the start point for any loops
:begin

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

# Start moving forwards
FORWARD()

# Start the detection
Goto(detect) # This line is redundant



# Detection code

# Set a label for loops and gotos
:detect

# Use random numbers if in test mode
IF ($testmode=0)
  $currentdistance = GetPing($pingport, $echoport)
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
ENDIF

  # Check to see if to reverse before turning
IF ($reverseturn = 1)
  # If the option of reverse before turning is set reverse
  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
  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
  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($sweepservo,$sweepmin)
  # Use random numbers if in test mode
  IF ($testmode=0)
    $pingmin = GetPing($pingport, $echoport)
  ELSE
    $pingmin = GetRandom(0,255)
  ENDIF

    # Wait
  Sleep(400)

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

    # Wait
  Sleep(400)

  # Move and check the center
  Servo($sweepservo,$sweepcenter)
  IF ($pingmin > $pingmax)
    Goto(moveright)
    FORWARD()
  ELSE
    Goto(moveleft)
    FORWARD()
  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($sweepservo, $sweepcenter)

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

  # 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($sweepservo, $sweepmax)

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

  # 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($sweepservo, $sweepmin)

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

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

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

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

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

  # Else something has gone wrong
ELSE
  # Set an error flag for debugging purposes
  $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($sweepservo,$sweepmin)

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

# Wait
Sleep(200)

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

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

# Wait
Sleep(200)

# Move the servo back to the center
Servo($sweepservo,$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
  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
  FORWARD()

  # Else they are both the same
ELSE

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

  # And move forwards again
  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
$BoxedInRun = 1

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

# Use random numbers if in test mode
IF ($testmode=0)
  $side1scan = GetPing($pingport,$echoport)
ELSE
  $side1scan = GetRandom(0,255)
ENDIF

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

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

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

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

  # Check if boxed in by comparing 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($sweepservo,$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($pingport,$echoport)
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
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)

  # 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 again but escape from a loop
  Goto(escape)

  # Reset the last move
  $lastmove = "none"

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

  # Save the penultimate move
  $penultimatemove = $lastmove

  # Save the last move
  $lastmove= "left"

ENDIF

  # Go back to the main script
Return()

# End of scripts