Autonomous Roaming V1.2.0

# Autonomous roaming
# Using Ultra Sonic Ping Sensor on servo for avoidance
# Version 1.2.0
# Last Updated: 23-07-2013
# Author: Rich Pyke
# Language: EZ-Script 2013.07.22 or later
# www.EZ-Robot.com


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

# Adjust values below for movement control
$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
#sweepcenter automatically calculated
$sweepservodelay = 500 # Change for delay between sweep servo movements and readings


# ------- Do not adjust these values -------
# Calculate the centre position for the sweeping servo
# Minimum position plus maximum position divided by 2 then 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

# Clear flag for IsBoxedIn
$isboxedin = 0 # Do not change

# Set last and penultimate moves.
$penultimatemove = "none"
$lastmove = "none"



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

# Set the start point for any loops
:begin

# Set the sweep servo speed
ServoSpeed(D2, 0)

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

# Start moving forwards
FORWARD()


# Start the detection

# ------- Detection code -------
# Set a label for loops and gotos
:detect

# Get distance
$currentdistance = GetPing(D0, D1)

# 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
IF ($isboxedin = 1)

  # Run the escape code
  Goto(escape)

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

# Otherwise just stop reverse and turn
ELSE

  # Stop and sleep before movement
  Stop()
  Sleep(200)

  # Reverse away
  Reverse($movementspeed,$reverseamount)

  # 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)

    # Stop and sleep before movement
    Stop()
    Sleep(200)

    # Move 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)

    # Stop and sleep before movement
    Stop()
    Sleep(200)

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

    # Get distance
    $pingmin = GetPing(D0, D1)

    # Wait
    Sleep(400)

    # Move and check the right side
    Servo(D2,$sweepmax)

    # Get distance
    $pingmax = GetPing(D0, D1)

    # Wait
    Sleep(400)

    # Move and check the center
    Servo(D2,$sweepcenter)

    # Check distances for left and right movement
    IF ($pingmin > $pingmax)

      # If left side is closer move right
      Goto(moveright)

      # Stop and sleep before movement
      Stop()
      Sleep(200)

      # Move forwards
      FORWARD()

    # Otherwise right side is closer
    ELSE
      # So move left
      Goto(moveleft)

      # Stop and sleep before movement
      Stop()
      Sleep(200)

      # Move forwards
      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)

ENDIF

# Return back to the main script
Return()



# ------- The sweep center code -------

# Set a label for loops and gotos
:sweepcenter

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

# Get distance
$pingmin = GetPing(D0, D1)

# Wait
Sleep(200)

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

# Get distance
$pingmax = GetPing(D0, D1)

# 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

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

# Get distance
  $side1scan = GetPing(D0, D1)

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

# Get distance
$side2scan = GetPing(D0, D1)

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

# Get distance
$centerscan = GetPing(D0, D1)

# 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

# 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
# Get distance
$escapescan = GetPing(D0, D1)

# 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

# Once clear stop and sleep before movement
Stop()
Sleep(200)

# Move 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)
  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 -------