root / trunk / code / projects / colonet / utilities / robot_wireless_relay / colonybfs.c @ 13
History | View | Annotate | Download (5.06 KB)
1 |
/*
|
---|---|
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 |
|
158 |
|