Several species of insects use the sun to maintain a correct heading in an effort to navigate from place to place, even as they have to adjust their paths to avoid things like bug zappers and swatters. This first part of the project asks you to emulate -- at least at a coarse level -- this insect behavior by implementing two path-planning algorithms. Appropriately, the algorithms are named Bug1 and Bug2.
The idea behind both algorithms is that your robot knows the direction in which it should head towards its goal, but it does not know anything about the obstacles that lie between its current position and that goal. The basic idea is to start heading toward the goal (since the correct direction is known) and see whether any obstacles present themselves. We will call the line between the robot's initial position and the goal the S-line.
If an obstacle is in the way of the robot, i.e., if it is interposed on the S-line between the starting point and the goal, the two Bug algorithms deal with that obstacle slightly differently:
This problem is to implement these two "Bug" algorithms using the wall-following code you wrote in the second half of Lab Project A as a starting point. As with that assignment, you can assume that the obstacles will consist of walls meeting at right angles (an indoor application). The S-line need not, however, be parallel or perpendicular to the obstacles' walls. In implementing these algorithms, there are a number of choices to make, e.g., which direction to start heading around an encountered obstacle. It doesn't matter which direction you choose, as long as you stay consistent. Another choice that comes up in Bug1 is which direction to return to the perimeter point closest to the goal. In that case, it is preferable to return along the shorter path, (though either way is acceptable for this assignment).
Maps, Initial Points, and Destination Points
To test your algorithms, there are two world maps available in /cs/cs154/as/B. For the first map (bugmapA), the goal point is (2750,4200); for the second map (bugmapB) it's (0,4700). For all of the test runs, the default starting point will be used, i.e., the position the robot takes after the zr() call. Both of these provided maps may be more complicated than you want to use on a first run to debug your algorithms -- feel free to try it out on your own maps, also.
In your write-up of this part of the assignment, be sure to include the following
Feel free to add examples in your own environments, too. Also, (optional) please comment on anything that would make this assignment better in the future... !
Our basic approach to the bug 1 problem was to divide the robot's actions
into 5 states. Here's the description of each state:
State 0 is very basic. It gets the current angle of the robot,
calculates the new desired angle based on the current location and the location
of the target, and turns the robot by the difference of the two. Then it goes to
In state 1 we check if the robot has reached the target. We check this by taking the difference between the current location and the target (for both x and y) and checking if it's below 30 (a tolerance to account for approximations). If so, the robot goes to state 4, in which it just waits for the "q" key to be pressed. If the robot reaches a wall, it stops and moves to state 2.
State 2 is the most complicated. The first thing we do is to correct the robot angle in order for it to be ABSOLUTELY parallel to the wall (this assumes right angles). We have two variables: direction - which holds the current direction of the robot (top&left, top&right, bottom&left or bottom&right) - and wall_direction (left of robot or right of robot). From these variables we calculate the desired angle so that the robot is parallel to the wall.
After turning into that direction, we record the current
location (original location) so that we can check when the robot gets back to
it. Then we call the wallFollow function until the robot is 30 units away from
the original point (because of our tolerance). After this we call wallFollow
again until the robot is back to the original point. During this time, we
calculate the new distance to the target. If smaller than the previous distance,
we record the new best point around the wall. When back to the original point,
the robot goes to state 3.
In state 3, the robot moves towards the best point recorded in state 2. This is done using wallFollow again. Once the robot reaches the best point, we simply throw it back into state 0.
LIMITATIONS: The main limitation of this code is that it assumes that the walls have right angles.
Here is the link to the actual code: bug1.cc
Here are the traces for map A and B:
The algorithm for bug2 is pretty straight foward. It follows the assignment nearly step by step. The Implementation is as followed:
Simply find the angle that the robot is facing and calculate the angle that the robot needs to be facing by taking the arctangent of the difference between and target and the robot coordinates.
Set tv to 200, and move robot foward until the shortest sonar reading is close to a wall.
Turn the robot until the wall is on the right side of it, and then call the wall follow code to move along the wall. Each step of the way, the robot checks to see if it is back on the S-line. It does this check by comparing the S-line slope and the and its present slope to the target. If the slopes are the same, then the robot is back on the S-line, and we need to check if the robot is closer to the target now before following the wall. It does so by calculating the distance to the target before the wallFollow started, and when it is back on the S-line again. If the robot is closer, then we exit the wall follow code.
Now the robot is back on the S-line, and it needs to turn to the target and move to it.
The robot can now loop through the steps 1-5 until it finds it self at the target.
LIMITATIONS: There are 2 limitations to this implementation. The first limitation is that the wall following code assumes straight walls with right angles. The code will still work otherwise, however it will not be very efficient. The second limitation is that the code has trouble with rounding decimals to integers. Thus there are slight estimation errors. The slopes from the robot to the target are not perfect numbers, and the encoders of the robot can only store integers. So estimation error occurs in two places; The first error occurs when the bug is checking if he is on the back on the S-line while following the wall. The bug needs to check his current slope to the original slope. The second place where estimation error comes in is when the robot checks if he is at the goal. Since the encoders can only track about every 3 coordinates, we need estimate again.
OVERALL: The bug code works well overall. The bug ends up within an inch of the target with each run.
Here is the link to the code: bug2.cc
Here are the acttal traces for map A and B: