Revision 305
updated lemmings - smoother leader transitions
branches/lemmings/code/behaviors/lemmings/lemmings.c | ||
---|---|---|
17 | 17 |
#define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern |
18 | 18 |
#define SPEED 200 // set default speed |
19 | 19 |
|
20 |
#define LEMMINGS_PAN 7 // set the lemmings PAN |
|
21 |
|
|
20 | 22 |
/* Globals */ |
21 | 23 |
static int cur_state; // current state |
22 | 24 |
static int local_leader; // follow this bot (-1 if leader) |
23 | 25 |
static int global_leader; // this is the global leader |
26 |
static int follow_lock; // this locks in the bot to follow |
|
24 | 27 |
static int follow_multi; // set to 0 for single-file following, 1 for tree following |
25 | 28 |
static int local_id; // this is the local robot id |
26 | 29 |
static int speed; // robot's forward speed |
27 | 30 |
static int turn_speed; // robot's turning speed |
31 |
static int pan; // set the lemmings PAN |
|
28 | 32 |
|
29 | 33 |
/* Internal prototypes */ |
30 | 34 |
static int get_local_leader(void); |
... | ... | |
46 | 50 |
cur_state = FOLLOW; |
47 | 51 |
local_leader = -1; |
48 | 52 |
global_leader = -1; |
53 |
follow_lock = 0; |
|
49 | 54 |
local_id = wl_get_xbee_id(); |
50 | 55 |
speed = SPEED; |
51 | 56 |
turn_speed = 0; |
57 |
pan = LEMMINGS_PAN; |
|
52 | 58 |
|
53 | 59 |
follow_multi = FOLLOW_MULTI_DEFAULT; |
54 | 60 |
} |
... | ... | |
76 | 82 |
leader_check_count++; |
77 | 83 |
if (leader_check_count % 3 == 0) { |
78 | 84 |
|
79 |
if (cur_state == LEAD && leader_check_count % 12 == 0) {
|
|
85 |
if (leader_check_count % 12 == 0) { |
|
80 | 86 |
// find robot with max id and assign as leader |
81 | 87 |
int max = -1; |
82 | 88 |
wl_token_iterator_begin(); |
... | ... | |
94 | 100 |
orb_set_color(BLUE); |
95 | 101 |
cur_state = LEAD; |
96 | 102 |
local_leader = -1; |
103 |
follow_lock = 0; |
|
97 | 104 |
speed = SPEED; |
98 | 105 |
turn_speed = 0; |
99 | 106 |
// usb_puts(" becoming leader \n"); |
... | ... | |
113 | 120 |
leader_bom = (local_leader>=0)?wl_token_get_sensor_reading(local_id, local_leader):-1; |
114 | 121 |
|
115 | 122 |
if (leader_bom < 0) { |
116 |
//usb_puts("no connection to leader\n"); |
|
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"); |
|
117 | 130 |
|
118 |
// Can't see our old leader -- get new bot to follow. |
|
119 |
local_leader = get_local_leader(); |
|
120 |
|
|
121 |
if (local_leader == -1) { |
|
122 |
/* if there is only one robot in the token ring, there is nothing to |
|
123 |
follow, so don't do anything */ |
|
124 |
/*motors_off(); |
|
125 |
speed = 0; |
|
126 |
return;*/ |
|
127 |
// nothing to follow, so become a local_leader and start leading |
|
128 |
//usb_puts("can't see anyone - becoming a leader\n"); |
|
129 |
cur_state = LEAD; |
|
130 |
local_leader = -1; |
|
131 |
orb_set_color(BLUE); |
|
132 |
speed = SPEED; |
|
133 |
turn_speed = 0; |
|
134 |
} else { |
|
135 |
//usb_puts("new leader is"); |
|
136 |
//usb_puti(local_leader); |
|
137 |
//usb_puts("\n"); |
|
138 |
// get data in order to follow leader |
|
139 |
leader_bom = wl_token_get_sensor_reading(local_id, local_leader); |
|
140 |
get_bom_velocity(leader_bom); |
|
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 |
} |
|
141 | 155 |
} |
142 | 156 |
} else { |
143 | 157 |
// Leader is still in sight, so follow it |
144 | 158 |
get_bom_velocity(leader_bom); |
159 |
follow_lock = 1; // lock in bot to follow |
|
145 | 160 |
} |
146 | 161 |
} |
147 | 162 |
} |
... | ... | |
225 | 240 |
int cur_weight; |
226 | 241 |
int best_weight = 1000; // set to infinity initially |
227 | 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; |
|
228 | 248 |
|
229 | 249 |
wl_token_iterator_begin(); |
230 | 250 |
|
Also available in: Unified diff