root / branches / lemmings / code / behaviors / lemmings / lemmings.c @ 305
History | View | Annotate | Download (7.64 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wl_defs.h> |
3 |
#include <wireless.h> |
4 |
#include <wl_token_ring.h> |
5 |
//#include <colonet_dragonfly.h>
|
6 |
//#include "smart_run_around_fsm.h"
|
7 |
#include "lemmings.h" |
8 |
|
9 |
/*A simple behavior for following the leader
|
10 |
SINGLE_FILE pattern not implemented yet
|
11 |
*/
|
12 |
|
13 |
// FSM states
|
14 |
#define LEAD 0 // be a leader |
15 |
#define FOLLOW 1 // follow another robot |
16 |
|
17 |
#define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern |
18 |
#define SPEED 200 // set default speed |
19 |
|
20 |
#define LEMMINGS_PAN 7 // set the lemmings PAN |
21 |
|
22 |
/* Globals */
|
23 |
static int cur_state; // current state |
24 |
static int local_leader; // follow this bot (-1 if leader) |
25 |
static int global_leader; // this is the global leader |
26 |
static int follow_lock; // this locks in the bot to follow |
27 |
static int follow_multi; // set to 0 for single-file following, 1 for tree following |
28 |
static int local_id; // this is the local robot id |
29 |
static int speed; // robot's forward speed |
30 |
static int turn_speed; // robot's turning speed |
31 |
static int pan; // set the lemmings PAN |
32 |
|
33 |
/* Internal prototypes */
|
34 |
static int get_local_leader(void); |
35 |
static int get_edge_weight(int robot1, int robot2); |
36 |
static void get_bom_velocity(int bom_id); |
37 |
|
38 |
void lemmings_init() {
|
39 |
// usb_puts("lemmings_init\n");
|
40 |
|
41 |
// colonet_init();
|
42 |
//run_around_init(); // prepare for moving
|
43 |
//range_init();
|
44 |
//analog_init();
|
45 |
/* range finder code interferes with token ring */
|
46 |
orb_init(); |
47 |
orb_enable(); |
48 |
|
49 |
// set to defaults
|
50 |
cur_state = FOLLOW; |
51 |
local_leader = -1;
|
52 |
global_leader = -1;
|
53 |
follow_lock = 0;
|
54 |
local_id = wl_get_xbee_id(); |
55 |
speed = SPEED; |
56 |
turn_speed = 0;
|
57 |
pan = LEMMINGS_PAN; |
58 |
|
59 |
follow_multi = FOLLOW_MULTI_DEFAULT; |
60 |
} |
61 |
|
62 |
/* The main function, call this to update states as frequently as possible. */
|
63 |
void lemmings_FSM(void) { |
64 |
int leader_bom = 0; |
65 |
static int leader_check_count = 0; |
66 |
|
67 |
|
68 |
if (wl_token_get_num_robots() <= 1) { |
69 |
/* if there is only one robot in the token ring, there is nothing to
|
70 |
follow, so don't do anything */
|
71 |
motors_off(); |
72 |
speed = 0;
|
73 |
orb_set_color(GREEN); |
74 |
return;
|
75 |
} |
76 |
|
77 |
//usb_puts("state: ");
|
78 |
//usb_puti(cur_state);
|
79 |
//usb_puts("\n");
|
80 |
|
81 |
// only check for new global leader once every 5 iterations
|
82 |
leader_check_count++; |
83 |
if (leader_check_count % 3 == 0) { |
84 |
|
85 |
if (leader_check_count % 12 == 0) { |
86 |
// find robot with max id and assign as leader
|
87 |
int max = -1; |
88 |
wl_token_iterator_begin(); |
89 |
while (wl_token_iterator_has_next()) {
|
90 |
int n = wl_token_iterator_next();
|
91 |
if (n > max) {
|
92 |
max = n; |
93 |
} |
94 |
/* usb_puti(n); */
|
95 |
/* usb_puts("\n"); */
|
96 |
} |
97 |
|
98 |
if (max == local_id) {
|
99 |
// become the global leader
|
100 |
orb_set_color(BLUE); |
101 |
cur_state = LEAD; |
102 |
local_leader = -1;
|
103 |
follow_lock = 0;
|
104 |
speed = SPEED; |
105 |
turn_speed = 0;
|
106 |
// usb_puts(" becoming leader \n");
|
107 |
} |
108 |
else {
|
109 |
// become a follower
|
110 |
orb_set_color(RED); |
111 |
cur_state = FOLLOW; |
112 |
// usb_puts(" becoming follower\n");
|
113 |
} |
114 |
|
115 |
global_leader = max; // set the global leader
|
116 |
} |
117 |
|
118 |
if (cur_state == FOLLOW) {
|
119 |
// Make sure we can see our local leader
|
120 |
leader_bom = (local_leader>=0)?wl_token_get_sensor_reading(local_id, local_leader):-1; |
121 |
|
122 |
if (leader_bom < 0) { |
123 |
if (follow_lock) {
|
124 |
// unlock bot to follow
|
125 |
follow_lock = 0;
|
126 |
// actually change to follow other bot on next loop
|
127 |
} |
128 |
else {
|
129 |
//usb_puts("no connection to leader\n");
|
130 |
|
131 |
// Can't see our old leader -- get new bot to follow.
|
132 |
local_leader = get_local_leader(); |
133 |
|
134 |
if (local_leader == -1) { |
135 |
/* if there is only one robot in the token ring, there is nothing to
|
136 |
follow, so don't do anything */
|
137 |
/*motors_off();
|
138 |
speed = 0;
|
139 |
return;*/
|
140 |
// nothing to follow, so become a local_leader and start leading
|
141 |
//usb_puts("can't see anyone - becoming a leader\n");
|
142 |
cur_state = LEAD; |
143 |
local_leader = -1;
|
144 |
orb_set_color(BLUE); |
145 |
speed = SPEED; |
146 |
turn_speed = 0;
|
147 |
} else {
|
148 |
//usb_puts("new leader is");
|
149 |
//usb_puti(local_leader);
|
150 |
//usb_puts("\n");
|
151 |
// get data in order to follow leader
|
152 |
leader_bom = wl_token_get_sensor_reading(local_id, local_leader); |
153 |
get_bom_velocity(leader_bom); |
154 |
} |
155 |
} |
156 |
} else {
|
157 |
// Leader is still in sight, so follow it
|
158 |
get_bom_velocity(leader_bom); |
159 |
follow_lock = 1; // lock in bot to follow |
160 |
} |
161 |
} |
162 |
} |
163 |
else {
|
164 |
// prevent spinning in circles
|
165 |
turn_speed /= 5;
|
166 |
} |
167 |
|
168 |
// check if running into something
|
169 |
/*int range = range_read_distance(IR2);
|
170 |
if (range > 0 && range < 150) {
|
171 |
if (cur_state == LEAD) {
|
172 |
turn_speed = 100; // turn to avoid
|
173 |
}
|
174 |
else {
|
175 |
speed = 0; // stop to avoid
|
176 |
}
|
177 |
}*/
|
178 |
|
179 |
// do movement
|
180 |
move(speed, turn_speed); |
181 |
} |
182 |
|
183 |
// translate bom reading into direction and speed
|
184 |
static void get_bom_velocity(int bom_id) { |
185 |
int error = 0; |
186 |
|
187 |
if (bom_id < 0 || bom_id > 15) { |
188 |
// usb_puts("ERROR - invalid bom_id\n");
|
189 |
turn_speed = 0;
|
190 |
speed = 0;
|
191 |
return;
|
192 |
} |
193 |
|
194 |
// set so right is negative, left is positive
|
195 |
if (bom_id <= 12) { |
196 |
error = bom_id - 4;
|
197 |
} else {
|
198 |
error = bom_id - 20;
|
199 |
} |
200 |
|
201 |
// set turn speed and main speed
|
202 |
// if error is small, turn slowly and travel at normal speed
|
203 |
// if error is large, turn faster and travel at reduced speed
|
204 |
switch(error)
|
205 |
{ |
206 |
case 0: |
207 |
turn_speed = 0;
|
208 |
speed = SPEED; |
209 |
return;
|
210 |
case 1: |
211 |
case -1: |
212 |
turn_speed = error * 5;
|
213 |
speed = SPEED; |
214 |
return;
|
215 |
case 2: |
216 |
case -2: |
217 |
turn_speed = error * 8;
|
218 |
speed = SPEED; |
219 |
return;
|
220 |
case 3: |
221 |
case -3: |
222 |
turn_speed = error * 12;
|
223 |
speed = SPEED; |
224 |
return;
|
225 |
case 4: |
226 |
case -4: |
227 |
turn_speed = error * 16;
|
228 |
speed = SPEED - SPEED * error * error / 36;
|
229 |
return;
|
230 |
} |
231 |
// if not in a case
|
232 |
turn_speed = error * 20;
|
233 |
speed = SPEED - SPEED * error * error / 36;
|
234 |
} |
235 |
|
236 |
// create connected components and pick a leader for each chain
|
237 |
// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
|
238 |
static int get_local_leader(void) { |
239 |
int nodeB;
|
240 |
int cur_weight;
|
241 |
int best_weight = 1000; // set to infinity initially |
242 |
int best_node = -1; |
243 |
|
244 |
// check for global leader first
|
245 |
int test = wl_token_get_sensor_reading(local_id, global_leader);
|
246 |
if (test != -1) |
247 |
return global_leader;
|
248 |
|
249 |
wl_token_iterator_begin(); |
250 |
|
251 |
// iterator through robots in token ring
|
252 |
while (wl_token_iterator_has_next()) {
|
253 |
nodeB = wl_token_iterator_next(); |
254 |
if (nodeB == local_id) {
|
255 |
continue; // can't follow self |
256 |
} |
257 |
|
258 |
// get an edge weight based on the robot position
|
259 |
cur_weight = get_edge_weight(local_id, nodeB); |
260 |
|
261 |
if (nodeB == global_leader && cur_weight != -1) { |
262 |
// always follow the global leader if you have the option to
|
263 |
best_node = nodeB; |
264 |
break;
|
265 |
} |
266 |
|
267 |
if (cur_weight >= 0 && cur_weight < best_weight) { |
268 |
// this is new best node, so save values.
|
269 |
best_weight = cur_weight; |
270 |
best_node = nodeB; |
271 |
} |
272 |
} |
273 |
|
274 |
return best_node;
|
275 |
} |
276 |
|
277 |
// get edge weight of sensor matrix
|
278 |
// add in BOM range data when BOM 1.5 comes out
|
279 |
static int get_edge_weight(int robot1, int robot2) { |
280 |
int bom = wl_token_get_sensor_reading(robot1,robot2);
|
281 |
|
282 |
// Robots closer to directly in front of us have lower weight.
|
283 |
switch(bom) {
|
284 |
case 12: |
285 |
return 10; |
286 |
|
287 |
case 13: |
288 |
case 11: |
289 |
return 9; |
290 |
|
291 |
case 14: |
292 |
case 10: |
293 |
return 8; |
294 |
|
295 |
case 15: |
296 |
case 9: |
297 |
return 7; |
298 |
|
299 |
case 0: |
300 |
case 8: |
301 |
return 6; |
302 |
|
303 |
case 1: |
304 |
case 7: |
305 |
return 5; |
306 |
|
307 |
case 2: |
308 |
case 6: |
309 |
return 4; |
310 |
|
311 |
case 3: |
312 |
case 5: |
313 |
return 3; |
314 |
|
315 |
case 4: |
316 |
return 2; |
317 |
} |
318 |
|
319 |
return -1; |
320 |
} |