GVG Algorithm Outline

          Initialization:

          The program connects to the robot and initialize the encoders and 
          sensors. The datastructures for mapping and exploration are also 
          initialized at this time.

          Access GVG:

          In order to access the nearest GVG edge in the environment, the 
          sonar sensors are polled to determine local minima. The robot 
          then turns away from the nearest object and moves forward, checking
          local minima. When equidistance is reached the robot stops and turns
          onto the calculated GVG edge. Now the main loop of the algorithm may
          begin.

          Edge Trace:

          This is a velocity based control loop. The sonar sensor data is 
          processed using the arc transversal method(ATM) to refine the 
          coordinates of local minima. The local minima coordinates are used
          to calculate the GVG edge, which is a line that can be followed by 
          adjusting the robot velocities appropriately. Most of the time spent
          in this stage of the algorithm, the robot follows the GVG edge defined
          by the closest pair of local minima.

          When the local minima indicate that the robot is approaching a meetpoint,
          control is passed to the Meetpoint Homing control law.

          When the local minima indicate that the robot is approaching a boundary 
          node, control is passed to the Boundary Registration 
          procedure.
          

          Meetpoint Homing:

          This is a short control loop that calculates a desired coordinate position
          for the robot based on three or more local minima. This coordinate is the 
          equidistant point of all local minima and is referred to as a meetpoint. 
          This control law is very important because the meetpoint is a stable 
          feature of the environment, and the robot is able to consistently move to 
          the coordinates with great accuracy.

          Once the robot halts at the coordinates of the meetpoint, control passes 
          to Meetpoint Registration.

          Meetpoint Registration:

          The sensor data at a meetpoint defines a signature for that meetpoint. 
          Using the signature with GVG connectivity information, the meetpoint 
          can be classified as a new point in the environment or a previously 
          explored coordinate. In both cases this information is stored and 
          control is then passed to the Determine Direction function.

          Boundary Registration:

          When a boundary node is detected the robot halts and registers the node 
          information.  This function is similar to Meetpoint Registration and 
          differs in that no homing takes place. Control is passed to the 
          Determine Direction function.

          Determine Direction:

          This function analyzes the stored data and determines which edge of 
          the robot's current node should be explored. When all of the current 
          node edges are explored, a search by Djykstra's algorithm 
          (breadth-first-search) on the GVG determines the closest node with 
          unexpolored edges and which edge the robot should follow to reach 
          that node.

          If a departing edge is calculated, then the robot is rotated onto 
          that edge and control is passed to Edge Trace.

          If the search determines that the GVG has been completely explored, 
          then the algorithm is Finished.