Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- # 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
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement