## The buggy butler

### Part 1 -- The bug algorithms

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 algorithms

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:

• Bug1 specifies that the robot should circumnavigate the entire obstacle. As it does so, it should remember whatever point along the obstacle's perimeter is closest to the goal. (The insect analogy would be a scent that grows stronger with proximity to a nest or meal... .) Once the robot has returned to (approximately) the point at which it originally hit the obstacle, it continues following the perimeter until it returns to that remembered closest point. Upon reaching that remembered point, it departs the obstacle and continues along a new line towards the goal. This process repeats if there are other obstacles in the way.

• Bug2 takes a more aggressive approach. If an obstacle presents itself along the S-line, the robot again "wall-follows" along the perimeter of that obstacle. Rather than circumnavigating the obstacle, however, Bug2 specifies that as soon as the robot reaches another point along the S-line that is closer than the original point of contact with the obstacle, the robot should leave the obstacle perimeter and continue heading toward the goal. This procedure repeats for any additional obstacles that arise.

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.

Write-Up

In your write-up of this part of the assignment, be sure to include the following

• a general explanation of any problems you encountered and how you dealt with them (There are at least a couple that may not be evident from the algorithms' descriptions.)
• an evaluation of how well your bug algorithms work
• example traces of the robot

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... !

Results bug1

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: The robot turns in the direction of the target;
• State 1: Robot moving towards target at maximum speed (set by tv constant);
• State 2: Robot reached a wall. Calling wallFollow function to circumnavigate around the wall.
• State 3: The robot is back at the original point before calling wallFollow... Move towards best point around wall, then go back to State 0 again.
• State 4: Robot found target.

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 state 1.

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:

Results bug2

The algorithm for bug2 is pretty straight foward. It follows the assignment nearly step by step. The Implementation is as followed:

• Step 1: The robot turns in the direction of the target;

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.

• Step 2: Robot moving towards target at maximum speed (set by tv constant);

Set tv to 200, and move robot foward until the shortest sonar reading is close to a wall.

• Step 3: Robot reached a wall. Calling wallFollow function to circumnavigate around the 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.

• Step 4: The robot finds itself at another point on the S-line closer to the target then it originally was before calling the wallFollow function.

Now the robot is back on the S-line, and it needs to turn to the target and move to it.

• Step 5: Repeat Step 1-5 until robot finds itself at the target.

The robot can now loop through the steps 1-5 until it finds it self at the target.

• Step 6: The robot is at the target, and it stops.

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: