Revision 305
updated lemmings - smoother leader transitions
| 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