IR Roam V1.0.0

# IR Object Avoidance
# Using IR Sensor on servo for avoidance
# Based on Ping Roam V1.1.2
# Version 1.0.0
# Updated 2013-07-26
# Author Rich
# Software EZ-Builder 2013-07-24
# www.EZ-Robot.com


# Configuration Settings
# This script assumes the following ports;
# IR Sensor = ADC0
# Sweep Servo = D2


# Adjust values below for movement control
$maxdistance = 30 # Change for closest distance to object before avoiding in units
$boxedindistance = 50 # Change for closest distance to object 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


# --------------------- 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
$isboxedin = 0 # Do not change
# Set last and penultimate moves.
$penultimatemove = "none"
$lastmove = "none"
# ----------------------------------------------------------------------


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

# Start moving forwards
FORWARD()


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

# Get the current distance
$currentdistance = GetADC(ADC0)

  # Check the current distance against the max allowed distance
IF ($currentdistance >= $maxdistance)
  # If the current distance is above 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
ELSE

  # Stop and reverse
  Stop()
  Sleep(200)
  Reverse($movementspeed,$reverseamount)

  # Check the servo position
  # Check if it's to the left
  IF ($sweepcurrent = $sweepmin)
    # If the servo is in the lowest 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)

    # Get the current distance
    $irmin = GetADC(ADC0)

    # Wait
    Sleep(400)

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

    # Get the current distance
    $irmax = GetADC(ADC0)

    # Wait
    Sleep(400)

    # Move and check the center
    Servo(D2,$sweepcenter)
    IF ($irmin < $irmax)
      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)

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 the current distance
$irmin = GetADC(ADC0)

# Wait
Sleep(200)

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

# Get the current distance
$irmax = GetADC(ADC0)

# 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 ($irmin < $irmax)

  # 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 ($irmin > $irmax)

  # 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 the current distance
$side1scan = GetADC(ADC0)

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

# Get the current distance
$side2scan = GetADC(ADC0)

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

# Get the current distance
$centerscan = GetADC(ADC0)

# 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 the current distance
$escapescan = GetADC(ADC0)

# If the scan result is above 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