Maze-mapping and navigating robot

From RoboCup

Jump to: navigation, search

Beskrivelse

Maze-mapping robot bør bevege seg i en labyrint og lage kart av labyrint. Etterpå bør robot navigere gjennom labyrinten med bruk av kart.

Medlemmer

Første trinn

Image:labyrint_first_prototype.jpg

Første prototyp med Cerebot

Vi har bygget først enkelt robot med LEGO motorer og sensorer og Cerebot. Robot har en motor på hver side, og tre lys-sensorer foran. Vår første mål er at robot skal bevege seg på labyrint som består av svart tape på hvit papir. Robot skal bruke tre sensorer, og bør finne vei fra start til mål. Mål er vist med grønn farge. Seinere vil vi at robot også lager kart av labyrinten.

Eksempel-labyrint er vist til høyre.

Image:enkelt_labyrint.gif

Setup program

For å begynne å programmere må alt kobles riktig, dvs. motor driver, alle motor koblinger (EN - enable, DIR - direction), og modul for LEGO aktive sensorer (POWER_ON_SENSORS, ADCs). Koblinger er vist på følgende figur:

Image:labyrint_robot_prototype.gif

Prosjekt for WinAVR (inkluderer alt man trenger fra AVRlib): labyrint_setup.zip.

Program tillater å styre motorer i forskjellige retniger og hastighet, og lese fra de 3 lyssensorer.


Algoritme

The algorithm for traversing the maze will use the following idea:

The robot will use the timing to determine its position (only 90-degree turns exist in the maze). There are 14 different types of crossings:

  • "dead-end" in 4 orientations (coded as 1,2,4,8)
  • L shape (corner) in 4 orientations (coded as bit combinatios, see below)
  • T shape in 4 orientations
  • and maze target (specialcode)

Starting at initial location the robot will follow a line until the next crossing. At each crossing, it will determine the type of crossing and update its internal representation (map) of the maze. It will remember:

  • which crossings are interconnected in which directions
  • what are the lenghts of these connections
  • the coordinates of the crossings
  • the type of all discovered crossings
  • for each crossing, the number of the crossing from where it was discovered (for backtracking)

For this purpose the program will use the following data structures:

  • number of current (or last visited) crossing (integer)
  • current orientation of the robot (N,E,S,W)
  • x,y coordinates of all discovered crossings (arrays of integers)
  • type of each crossing (array of integers)
  • connections leading from each crossing in all directions N,W,S,W: i.e. the number of crossings that can be reach in each of the four directions, -1 if path not available (four integers for each crossing, i.e. 4 integer arrays)
  • the lenghts of the connections leading from each crossing in all four directions (or -1 if not available, 4 integer arrays)
  • the number of crossing from where each crossing was discovered (an integer array)

The traversing algorithm will utilize the following subroutines:

  • find_next_crossing() - the function will follow the line until a crossing is detected. It will determine the distance to that crossing and the type of the new crossing. It will place the robot in the centre of the newly discovered crossing.
  • turn_to(new_direction) - the function is called when the robot is at some crossing and wants to turn into a new direction

The algorithm itself will follow this recipe:

  • initialize the variables
    • empty the map,
    • set current crossing to 0,
    • set the type of 0 crossing to dead-end with exit at North,
    • set current orientation of the robot to North,
    • set current coordinates to 0,0.
  • repeat the following steps:
    • follow to the next crossing
    • update the current coordinates based on the distance travelled
    • if this is the target crossing, finish the repeat loop
    • determine if this is already a known crossing (using the current coordinates, with some tolerance for error!)
    • if this is an old crossing or dead end, retract to the last unvisited path and continue with the repeat loop again
    • otherwise, this is a new crossing, update the map:
      • give the crossing new number
      • store the coordinates and type of the crossing
      • store the connections in both directions betwenn the new and the last-visited crossing including the lengths
    • update the current coordinates, the current crossing, and choose new orientation
    • turn_to the new orientation and update the current orientation
    • continue the repeat loop

The algorithm utilizes a subroutine that will retract to the last unvisited path that will consist of these steps:

  • determine the previous crossing (from where the current one was discovered)
  • if there is no previous crossing, target was not found and the algorithm will stop
  • otherwise turn to the previous crossing
  • follow to the next crossing
  • if the current crossing contains a path that was not used yet, enter that path, and return
  • otherwise, continue from the start of the retract subroutine

Thus our efforts should start with building the robot (this step is now completed), implementing and testing the subroutines, and finally implementing and testing the whole algorithm.


Subrutiner

Detailed top-view of the robot:
Image:maze_robot_top_view_small.jpg
(+)

And how it finds its first maze crossing: video.


We have now decided on encoding of the directions:

Absolute:       Relative:
 North: 1        Forward: 1
 East:  2        Right:   2
 South: 4        Back:    4
 West:  8        Left:    8

Then we will need a couple of functions that will work with the directional information, which we can encode with efficient bit-shift operators:

 right(direction)    = (15 & (direction << 1)) | (direction >> 3)
 left(direction)     = (direction >> 1) | ((direction & 1) << 3)
 opposite(direction) = (15 & (direction << 2)) | (direction >> 2)

We have a function that determines the crossing type (as bitwise combination of possible directions) and we would like to determine the absolute crossing type code. We also know that there is a connection in backward direction because that is where we've just arrived from.

 int absolute(int relative, int robot)
 {
     int a = opposite(robot);
     if (relative & 1) a |= robot;
     if (relative & 2) a |= right(robot);
     if (relative & 8) a |= left(robot);
     return a;
 }

Finally, we will need a procedure that will turn the robot to a new desired absolute direction if we know the current crossing type and current robot direction:

 turn_to(robot, crossing_type, new_direction)
 {
     int count = 1;
     int rotate_direction = 1;     /* 1: right, 2: left */
     if (robot == new_direction) return;
     else if (right(robot) == new_direction) rotate_direction = 1;
     else if (left(robot) == new_direction) rotate_direction = 2;
     else if (crossing_type & right(robot)) count = 2; 
     
     while (count--) rotate_until_line(rotate_direction);
 }

Now we will test the turn_to procedure, and then we are ready for implementation of the whole program!

Software

maze.zip Program som har ny funksjon som følger linjen og finner neste kryss.


Personal tools