Asked
— Edited
V1.1.2 available on page 3
Taking on board some of the comments made recently regarding autonomous roaming, plus a few improvements I wanted to make I have updated the Ping Roam script (which if I knew it would be so popular I would have made more of an effort the first time around).
I've done some quick testing with it however my testbot runs on 4 modified servos with wheels on which isn't the best method of driving it so it may still have some bugs in... that's where you come in
Let me know if there are any bugs, anything that doesn't work quite like it should, any further improvements you want...
I did have a bug in the escape routine, which I think I have now solved. Where it has a loop in the routine to continue turning until the sensor reading is above the setpoint I think that was throwing off the return at the end of the routine, not returning to where it came from but returning to when it was told to loop. Solved with additional labels and it seems to be working now...
Changes
Boxed In check added
Escape from Boxed In added
Avoidance of left/right/left/right loops
Automatic calculation of sweep servo center position
Tidied up code
More comments added
Almost completely made with labels and gotos
And, here is the code
# 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
# Retyrn 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 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
# 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 afain 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
It's also on the cloud PingAvoidanceV1.1.0
And for ease of copying and pasting (in case the forums decide to add in some emoticons) it's also on pastebin
Any and all feedback is greatly appreciated no matter how trivial. Feel free to make your own improvements and modifications (let me know what they are too - believe it or not I'm still learning too).
That's allot of code
, I'll check this out. Thanks for sharing!
Bookmarked. Boxbot's upgrade to treads should be done this weekend, and prior to the treads, getting boxed in was a major issue, so i will definitely be trying this out.
Alan
@Derrick it's only 477 lines... a third are comments and a third are blank so it's not that long really
It looks bigger than it is. Not forgetting each GetPing has been expanded to 5 lines rather than 1 line since it has the "testmode" option (my way of being able to work on it and test it without it being connected to a robot).
It should be pretty easy to adjust now it has the comments for pretty much every command. There are two reasons for that, the first one is to help me debug it and solve the return problem I was getting for around a week before I realised my mistake. The second is to make it easy to understand, follow and modify. I will need to modify it to use it in Melvin since he will be using IR sensors and won't have a sweep servo but run 3 sensors in fixed positions. The logic and principles behind it will be the same though.
And the labels and gotos have been used so additional features can easily be thrown into it, and added as options with variables in the config section at the top. Or at least additional features could be added if I could think of any
Hopefully this evolves into a complex yet easily customisable autonomous roaming/navigation script.
i like the comments,it makes it so much easer down the road
The comments come in very handy when writing it over a few weeks. I'd get back to it and have to follow it all through again to see where I got to, why things were like they are etc. They also come in very handy when debugging it (and will come in handy when ironing out any bugs that may have gone un-noticed).
They will also come in handy when new features need adding in. If you can think when the feature needs to be executed you can find the place to throw in the goto really easily
And most of all, hopefully it further explains and demonstrates to people how scripts are made and work.
@rich, i have tried out the script. My setup on my platform is 1 motor with gearbox connected to an h bridge, 1 servo for left and right, and 1 ultrasonic sensor with servo. I changed the ports for my setup and merged my custom movement control to your script. Im kinda scratching my head because when i start your script everything seams fine, it sweeps and gets ping then movers forward. When it comes close to its first obstacle, it runs into it, then trys to move left and right, and then it just starts backing up forever. I don't know if its just my setup or what.my robot has issues.... Lol. I did not change anything besides the sensor ping, echo and servo sweep ie d10 trigger d11 echo and d8 sweep. Also i tried changing the max distance and turn amount. No luck....
send back a pic of the variable values when it gets stuck on a wall again.
I think my Robie may be cursed.
I've been anxious to give this code a try since @Rich posted the update. I added a few parameters for the head tilt since my Ping sensor is in my robot's head.
I loaded the code, made my change, set up a quick test course in my house, set Robie on the floor and hit start. He moved 6 inches and disconnected. I forgot to charge the batteries. (Doh!)
Once they were charged up, I set Robie on the floor and pressed start. He traveled a foot, then stopped. ARC locked up.
Back to the work bench and force closed ARC. Relaunched the software, ran a few minor tests. Everything looked fine.
Set Robie on the floor, hit start, and he started moving forward. Sweep motion worked great...and kept working like a champ as he drove right into the first obsticle and didn't stop. Stop script didn't stop him either, but I finally stopped him with the motion panel. No damage to the robot, but my ego took a thumping.
Back to the work bench and it appears my Ping sensor is goofed. It's reading a constant "255" and I'm not hearing that "ticking" sound it makes when active. By the time I checked the ports and swapped out two servo extentions, my free time was consumed, so I haven't had a chance to fully Scooby-Doo the problem. I'm probably going to have to crack open the head and check the solder points on the sensor itself...which will suck.
Still, I love the code and think it's going to work great for my little Robie!