IR Roam V0.0.1 Alpha

# IR Object Avoidance
# Using 3xIR sensors
# Version 0.0.1 Alpha
# 2013.04.22
# Author: Rich Pyke
# Language: EZ-Script

# Use at your own risk. This code is untested.


# Configuration Settings

# Adjust values below for port configuration
$leftir = ADC5
$rightir = ADC6
$centerir = ADC7

# 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 = 70 # Change for slow turn speed
$boxedinturntime = 250 # Change for how long to turn when checking if boxed in
$thinkdelay = 200 # Change for how long to sleep emulating thinking and reducing load

# Adjust values below for scan configuration
$scandelay = 500 # Change for delay between readings

# Testing Options
$testmode = 1 # Change to 1 if testing without sensors



# Do not adjust these values
$SweepErrorFlag = 0 # Do not change
$BoxedInErrorFlag = 0 # Do not change
$BoxedInRun = 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

# Start moving forwards
FORWARD($movementspeed)

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



# Detection code

# Set a label for loops and gotos
:detect

# Get the sensor readings
Goto(getreadings)

# Check the current distance against the max allowed distance
IF ($currentdistanceleft >= $maxdistance or $currentdistanceright >= $maxdistance or $currentdistancecenter >= $maxdistance)
  # If the current distance is higher the max distance start avoiding
  GOTO(avoid)

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

  # Wait
SLEEP ($scandelay)

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

    # Check to see if to reverse before turning
  IF ($reverseturn = 1)

    # If the option of reverse before turning is set reverse
    Stop()
    Sleep($thinkdelay)
    Reverse($movementspeed,$reverseamount)
  ENDIF

    # Check the closest side
    # Check if it's to the left
  IF ($currentdistanceleft > $currentdistanceright and $currentdistanceleft > $currentdistancecenter)
    # If left move right
    Goto(moveright)
    # Continue moving forwards
    Stop()
    Sleep($thinkdelay)
    FORWARD($movementspeed)

    # Else check if it's to the right
  ELSEIF ($currentdistanceright > $currentdistanceleft and $currentdistanceright > $currentdistancecenter)
    # If right move left
    Goto(moveleft)
    # Continue moving forwards
    Stop()
    Sleep($thinkdelay)
    FORWARD($movementspeed)

    # Else assume it's in the middle
  ELSE
    # If it is in the center check which side is closest to the object and move the other way
    # Check which is lowest and move the other way
    IF ($currentdistanceleft > $currentdistanceright)
      Goto(moveright)
      Stop()
      Sleep($thinkdelay)
      FORWARD()
    ELSE
      Goto(moveleft)
      Stop()
      Sleep($thinkdelay)
      FORWARD()
    ENDIF
  ENDIF
ENDIF
  # Return to the main code
Goto(avoidreturn)



# 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 to the left
Left($movementspeed,$boxedinturntime)

# Use random numbers if in test mode
IF ($testmode=0)
  $leftscan = GetADC($centerir)
ELSE
  $leftscan = GetRandom(0,255)
ENDIF

  # Get distance to the other side
  # Move to the right
Right($movementspeed,$boxedinturntime)
# Do it twice to get past the center
Right($movementspeed,$boxedinturntime)

# Use random numbers if in test mode
IF ($testmode=0)
  $rightscan = GetADC($centerir)
ELSE
  $rightscan = GetRandom(0,255)
ENDIF

  # Get distance to the front
  # Move to the center
Left($movementspeed,$boxedinturntime)

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

  # Check if boxed in by comparing the results against a fixed boxed in distance
IF ($leftscan > $boxedindistance and $rightscan > $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

# Turn slowly
Left($slowturn)

# Set up a loop
:escapeloop

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

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

  # Pause to reduce resource usage and comms bottleneck
  Sleep(100)

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

ENDIF

  # Continue forwards
Stop()
Sleep($thinkdelay)
FORWARD($movementspeed)

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



# Get readings code

:getreadings
# Use random numbers if in test mode
IF ($testmode=0)
  $currentdistanceleft = GetADC($leftir)
  $currentdistanceright = GetADC($rightir)
  $currentdistancecenter = GetADC($centerir)
ELSE
  $currentdistanceleft = GetRandom(0,255)
  $currentdistanceright = GetRandom(0,255)
  $currentdistancecenter = GetRandom(0,255)
ENDIF

# Go back to the main script
Return()



# Camera Tracking Code

:glyphdetection
# Check if glyph has been tracked
IF ($cameraistracking = 1)
  # Do something - not sure what  yet
ENDIF

# Go back to the main script
Return()

# End of scripts