We are using sonar as the primary method of obstacle detection. The sonar is mounted at the top of the robot.

Our wandering algorithm works on three basic conditions:

The turning behavior and emergency behavior have two states - a normal state and an alternate state for when the normal state doesn't work. In the turning behavior, the alternate state is used to straighten the robot to move along the corridor if it is repeatedly running into a wall. In the emergency behavior, the robot turns in small increments looking for an escape route in case it is cornered.

At present, the wandering algorithm does not actively look for openings to go into or use any non-random method to determine which way to turn, but it is structured so as to make these things easy to implement.

The pseudocode we created to do this

The actual implementation of this behavior is currently halted because if the Python client is connected to the robot server, the sonar server always returns a distance of 3. Here is a simple program to demonstrate this misbehavior.

The beginnings of an implementation of the emergency behavior can be found here. Note that the sonar is currently not calibrated in any meaningful way (the threshold is hard-coded), and that the servo is assumed to be facing forward.