root / trunk / code / projects / colonet / robot / wl_network_colonet / colonybfs.c @ 481
History | View | Annotate | Download (5.06 KB)
1 | 13 | emarinel | /*
|
---|---|---|---|
2 | Colony BFS
|
||
3 | |||
4 | Felix Duvallet
|
||
5 | |||
6 | */
|
||
7 | |||
8 | #include <stdio.h> |
||
9 | #include <string.h> |
||
10 | |||
11 | #include <firefly+_lib.h> |
||
12 | #include <ring_buffer.h> |
||
13 | |||
14 | #include "colonybfs.h" |
||
15 | |||
16 | |||
17 | |||
18 | #define NO_VAL 0 |
||
19 | |||
20 | |||
21 | RING_BUFFER_NEW(Queue, MAX_ROBOTS, char, q);
|
||
22 | |||
23 | /*
|
||
24 | colony_bfs_find_leader -- given an adjacency matrix, with a start and end
|
||
25 | robots, this will tell you which robot to follow next. If no path exists,
|
||
26 | it will return -1 (which may or may not go to 255 on the unsigned side
|
||
27 | of things -- be wary).
|
||
28 | |||
29 | It does find the entire path, perhaps someday I will make two function,
|
||
30 | where find_leader calls bfs(..) and only uses the first element.
|
||
31 | |||
32 | I ported this code from a MATLAB test file (also in the libs_general folder),
|
||
33 | and this has been tested on September 15, 2006 by Felix
|
||
34 | |||
35 | Todo: change NO_VAL to 255 and reflect those changes in the localization code
|
||
36 | |||
37 | Author: Felix Duvallet
|
||
38 | CMU Robotics Club, Colony Project
|
||
39 | |||
40 | */
|
||
41 | char colony_bfs_find_leader ( unsigned char matrix[MAX_ROBOTS][MAX_ROBOTS], |
||
42 | unsigned char start, unsigned char end ) { |
||
43 | |||
44 | |||
45 | /*
|
||
46 | //pseudocode for BFS
|
||
47 | |||
48 | procedure bfs(v)
|
||
49 | q := make_queue()
|
||
50 | enqueue(q,v)
|
||
51 | mark v as visited
|
||
52 | while q is not empty
|
||
53 | v = dequeue(q)
|
||
54 | process v
|
||
55 | for all unvisited vertices v' adjacent to v
|
||
56 | mark v' as visited
|
||
57 | enqueue(q,v')
|
||
58 | */
|
||
59 | |||
60 | // keep track of which nodes you have visited. Indexed by robot #
|
||
61 | // 1 if visited, 0 if not
|
||
62 | unsigned char visited_nodes[MAX_ROBOTS]; |
||
63 | // keep track of the distance from the start robot to other robots
|
||
64 | // also indexed by robot#
|
||
65 | unsigned char node_distances[MAX_ROBOTS]; |
||
66 | // this is the path you take
|
||
67 | unsigned char path[MAX_ROBOTS]; |
||
68 | |||
69 | //variables you will need
|
||
70 | unsigned char current_node; //current node |
||
71 | unsigned char current_neighbour_node; //neighbor to the current node |
||
72 | unsigned char current_neighbour_val; //value of that neighbour's sensors |
||
73 | unsigned char current_distance; //keep track of current distance to the start |
||
74 | unsigned char i; //counter variable |
||
75 | |||
76 | char large_number = 255; |
||
77 | |||
78 | //set visited nodes to all 0
|
||
79 | memset(visited_nodes, 0, MAX_ROBOTS);
|
||
80 | //set all the distances to a very large number
|
||
81 | //this isn't technically necessary, but it's probably a good thing -- just in case
|
||
82 | memset(node_distances, large_number, MAX_ROBOTS); |
||
83 | |||
84 | //set the path to all LARGE_NUMBER as well
|
||
85 | memset(path, large_number, MAX_ROBOTS); |
||
86 | |||
87 | RING_BUFFER_CLEAR(q); |
||
88 | |||
89 | //add the start node
|
||
90 | RING_BUFFER_ADD(q, start); |
||
91 | visited_nodes[start] = 1;
|
||
92 | node_distances[start] = 0;
|
||
93 | |||
94 | while(!RING_BUFFER_EMPTY(q)){
|
||
95 | RING_BUFFER_REMOVE(q, current_node); |
||
96 | |||
97 | //get the current distance from you to the start
|
||
98 | current_distance = node_distances[current_node]; |
||
99 | |||
100 | //this node is on your 'path'
|
||
101 | path[current_distance] = current_node; |
||
102 | //note: it's OK if this path leads nowhere -- the path will be
|
||
103 | //overwritten by a node that does lead to the goal
|
||
104 | //(if there is no path, we set path to null anyways)
|
||
105 | |||
106 | //from now on, all further nodes will be one further away -- increment
|
||
107 | current_distance++; |
||
108 | |||
109 | |||
110 | //process node -- basically check if it's the goal
|
||
111 | if(current_node == end) {
|
||
112 | //you reach the goal
|
||
113 | //return the first robot in the path, which is the one
|
||
114 | //to follow
|
||
115 | lcd_putchar('.');
|
||
116 | lcd_putchar('.');
|
||
117 | for(i = 0; i < MAX_ROBOTS; i++) |
||
118 | lcd_putchar(path[i] +48); //print out the path for fun -- remove this later |
||
119 | lcd_putchar('.');
|
||
120 | lcd_putchar('.');
|
||
121 | return path[1]; //path[0] is you. path[1] is who you want to follow |
||
122 | } |
||
123 | |||
124 | // go through all nodes adjacent to current
|
||
125 | // in effect, this means going through the current node's row in the matrix
|
||
126 | for(i = 0; i < MAX_ROBOTS; i++) { |
||
127 | //the robot # is actually just the index
|
||
128 | current_neighbour_node = i; |
||
129 | //the value is what is stored in the matrix (this is important)
|
||
130 | current_neighbour_val = matrix[current_node][i]; |
||
131 | |||
132 | //check for connected-ness and that it was not visited
|
||
133 | // unconnected visited
|
||
134 | if( (current_neighbour_val == NO_VAL) || (visited_nodes[current_neighbour_node]) ) {
|
||
135 | //if it is either unconnected or previously visited, exit
|
||
136 | continue; //go to the next node |
||
137 | } |
||
138 | |||
139 | //update the distance from the start node to this node
|
||
140 | node_distances[current_neighbour_node] = current_distance; |
||
141 | //also mark it as visited
|
||
142 | visited_nodes[current_neighbour_node] = 1;
|
||
143 | |||
144 | //also enqueue each neighbour
|
||
145 | RING_BUFFER_ADD(q, current_neighbour_node); |
||
146 | |||
147 | } |
||
148 | } |
||
149 | |||
150 | //if we get to here, there is no path
|
||
151 | memset(path, 0, MAX_ROBOTS);
|
||
152 | return -1; |
||
153 | |||
154 | |||
155 | |||
156 | } |
||
157 |