Revision 295
updated lemmings code
following is smoother, no spinning in circles
| lemmings.c | ||
|---|---|---|
| 23 | 23 |
static int global_leader; // this is the global leader |
| 24 | 24 |
static int follow_multi; // set to 0 for single-file following, 1 for tree following |
| 25 | 25 |
static int local_id; // this is the local robot id |
| 26 |
static int speed; // robot's forward speed |
|
| 27 |
static int turn_speed; // robot's turning speed |
|
| 26 | 28 |
|
| 27 | 29 |
/* Internal prototypes */ |
| 28 | 30 |
static int get_local_leader(void); |
| 29 | 31 |
static int get_edge_weight(int robot1, int robot2); |
| 30 |
static void get_bom_velocity(int bom_id, int* turn_speed, int* speed);
|
|
| 32 |
static void get_bom_velocity(int bom_id); |
|
| 31 | 33 |
|
| 32 | 34 |
void lemmings_init() {
|
| 33 | 35 |
// usb_puts("lemmings_init\n");
|
| 34 | 36 |
|
| 35 | 37 |
// colonet_init(); |
| 36 | 38 |
//run_around_init(); // prepare for moving |
| 39 |
//range_init(); |
|
| 40 |
//analog_init(); |
|
| 41 |
/* range finder code interferes with token ring */ |
|
| 42 |
orb_init(); |
|
| 43 |
orb_enable(); |
|
| 37 | 44 |
|
| 38 | 45 |
// set to defaults |
| 39 | 46 |
cur_state = FOLLOW; |
| 40 | 47 |
local_leader = -1; |
| 41 | 48 |
global_leader = -1; |
| 42 | 49 |
local_id = wl_get_xbee_id(); |
| 50 |
speed = SPEED; |
|
| 51 |
turn_speed = 0; |
|
| 43 | 52 |
|
| 44 | 53 |
follow_multi = FOLLOW_MULTI_DEFAULT; |
| 45 | 54 |
} |
| 46 | 55 |
|
| 47 | 56 |
/* The main function, call this to update states as frequently as possible. */ |
| 48 | 57 |
void lemmings_FSM(void) {
|
| 49 |
int leader_bom = 0, turn_speed = 0, speed = 0;
|
|
| 58 |
int leader_bom = 0; |
|
| 50 | 59 |
static int leader_check_count = 0; |
| 51 | 60 |
|
| 52 | 61 |
|
| ... | ... | |
| 54 | 63 |
/* if there is only one robot in the token ring, there is nothing to |
| 55 | 64 |
follow, so don't do anything */ |
| 56 | 65 |
motors_off(); |
| 66 |
speed = 0; |
|
| 67 |
orb_set_color(GREEN); |
|
| 57 | 68 |
return; |
| 58 | 69 |
} |
| 59 | 70 |
|
| ... | ... | |
| 63 | 74 |
|
| 64 | 75 |
// only check for new global leader once every 5 iterations |
| 65 | 76 |
leader_check_count++; |
| 66 |
if (leader_check_count % 5 == 0) {
|
|
| 67 |
wl_token_iterator_begin(); |
|
| 77 |
if (leader_check_count % 3 == 0) {
|
|
| 68 | 78 |
|
| 69 |
// find robot with max id and assign as leader |
|
| 70 |
int max = -1; |
|
| 71 |
while (wl_token_iterator_has_next()) {
|
|
| 72 |
int n = wl_token_iterator_next(); |
|
| 73 |
if (n > max) {
|
|
| 74 |
max = n; |
|
| 79 |
if (cur_state == LEAD && leader_check_count % 12 == 0) {
|
|
| 80 |
// find robot with max id and assign as leader |
|
| 81 |
int max = -1; |
|
| 82 |
wl_token_iterator_begin(); |
|
| 83 |
while (wl_token_iterator_has_next()) {
|
|
| 84 |
int n = wl_token_iterator_next(); |
|
| 85 |
if (n > max) {
|
|
| 86 |
max = n; |
|
| 87 |
} |
|
| 88 |
/* usb_puti(n); */ |
|
| 89 |
/* usb_puts("\n"); */
|
|
| 75 | 90 |
} |
| 76 |
/* usb_puti(n); */ |
|
| 77 |
/* usb_puts("\n"); */
|
|
| 78 |
} |
|
| 79 |
|
|
| 80 |
/* usb_puti(local_id); */ |
|
| 81 |
/* usb_puts(" is the local robot\n"); */
|
|
| 82 | 91 |
|
| 83 |
if (max == local_id) {
|
|
| 84 |
// become the global leader |
|
| 85 |
orb_set_color(BLUE); |
|
| 86 |
cur_state = LEAD; |
|
| 87 |
local_leader = -1; |
|
| 88 |
// usb_puts(" becoming leader \n");
|
|
| 89 |
} else {
|
|
| 90 |
// become a follower |
|
| 91 |
orb_set_color(RED); |
|
| 92 |
cur_state = FOLLOW; |
|
| 92 |
if (max == local_id) {
|
|
| 93 |
// become the global leader |
|
| 94 |
orb_set_color(BLUE); |
|
| 95 |
cur_state = LEAD; |
|
| 96 |
local_leader = -1; |
|
| 97 |
speed = SPEED; |
|
| 98 |
turn_speed = 0; |
|
| 99 |
// usb_puts(" becoming leader \n");
|
|
| 100 |
} |
|
| 101 |
else {
|
|
| 102 |
// become a follower |
|
| 103 |
orb_set_color(RED); |
|
| 104 |
cur_state = FOLLOW; |
|
| 93 | 105 |
// usb_puts(" becoming follower\n");
|
| 106 |
} |
|
| 107 |
|
|
| 108 |
global_leader = max; // set the global leader |
|
| 94 | 109 |
} |
| 110 |
|
|
| 111 |
if (cur_state == FOLLOW) {
|
|
| 112 |
// Make sure we can see our local leader |
|
| 113 |
leader_bom = (local_leader>=0)?wl_token_get_sensor_reading(local_id, local_leader):-1; |
|
| 95 | 114 |
|
| 96 |
global_leader = max; // set the global leader
|
|
| 97 |
}
|
|
| 115 |
if (leader_bom < 0) {
|
|
| 116 |
//usb_puts("no connection to leader\n");
|
|
| 98 | 117 |
|
| 99 |
switch (cur_state) {
|
|
| 100 |
case LEAD: |
|
| 101 |
// do lead action - move straight ahead |
|
| 102 |
orb_set_color(BLUE); |
|
| 103 |
move(SPEED, 0); |
|
| 104 |
break; |
|
| 105 |
|
|
| 106 |
case FOLLOW: |
|
| 107 |
orb_set_color(RED); |
|
| 108 |
|
|
| 109 |
// Make sure we can see our local leader |
|
| 110 |
leader_bom = (local_leader>=0)?wl_token_get_sensor_reading(local_id, local_leader):-1; |
|
| 111 |
|
|
| 112 |
if (leader_bom < 0) {
|
|
| 113 |
//usb_puts("no connection to leader\n");
|
|
| 114 |
|
|
| 115 |
// Can't see our old leader -- get new bot to follow. |
|
| 116 |
local_leader = get_local_leader(); |
|
| 117 |
|
|
| 118 |
if (local_leader == -1) {
|
|
| 119 |
// nothing to follow, so become a local_leader and start leading |
|
| 120 |
//usb_puts("can't see anyone - becoming a leader\n");
|
|
| 121 |
cur_state = LEAD; |
|
| 122 |
local_leader = -1; |
|
| 123 |
orb_set_color(BLUE); |
|
| 124 |
move(SPEED, 0); |
|
| 125 |
return; |
|
| 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); |
|
| 141 |
} |
|
| 126 | 142 |
} else {
|
| 127 |
//usb_puts("new leader is");
|
|
| 128 |
//usb_puti(local_leader); |
|
| 129 |
//usb_puts("\n");
|
|
| 130 |
// get data in order to follow leader |
|
| 131 |
leader_bom = wl_token_get_sensor_reading(local_id, local_leader); |
|
| 132 |
get_bom_velocity(leader_bom, &turn_speed, &speed); |
|
| 143 |
// Leader is still in sight, so follow it |
|
| 144 |
get_bom_velocity(leader_bom); |
|
| 133 | 145 |
} |
| 134 |
} else {
|
|
| 135 |
// Leader is still in sight, so follow it |
|
| 136 |
get_bom_velocity(leader_bom, &turn_speed, &speed); |
|
| 137 | 146 |
} |
| 138 |
|
|
| 139 |
//usb_puts("following robot# ");
|
|
| 140 |
//usb_puti(local_leader); |
|
| 141 |
//usb_puts(" bom of leader: ");
|
|
| 142 |
//usb_puti(leader_bom); |
|
| 143 |
//usb_puts("\n");
|
|
| 144 |
|
|
| 145 |
move(speed, turn_speed); // move in direction of leader |
|
| 146 |
break; |
|
| 147 | 147 |
} |
| 148 |
else {
|
|
| 149 |
// prevent spinning in circles |
|
| 150 |
turn_speed /= 5; |
|
| 151 |
} |
|
| 152 |
|
|
| 153 |
// check if running into something |
|
| 154 |
/*int range = range_read_distance(IR2); |
|
| 155 |
if (range > 0 && range < 150) {
|
|
| 156 |
if (cur_state == LEAD) {
|
|
| 157 |
turn_speed = 100; // turn to avoid |
|
| 158 |
} |
|
| 159 |
else {
|
|
| 160 |
speed = 0; // stop to avoid |
|
| 161 |
} |
|
| 162 |
}*/ |
|
| 163 |
|
|
| 164 |
// do movement |
|
| 165 |
move(speed, turn_speed); |
|
| 148 | 166 |
} |
| 149 | 167 |
|
| 150 | 168 |
// translate bom reading into direction and speed |
| 151 |
static void get_bom_velocity(int bom_id, int* turn_speed, int* speed) {
|
|
| 169 |
static void get_bom_velocity(int bom_id) {
|
|
| 152 | 170 |
int error = 0; |
| 153 | 171 |
|
| 154 | 172 |
if (bom_id < 0 || bom_id > 15) {
|
| 155 | 173 |
// usb_puts("ERROR - invalid bom_id\n");
|
| 156 |
*turn_speed = 0;
|
|
| 157 |
*speed = 0;
|
|
| 174 |
turn_speed = 0; |
|
| 175 |
speed = 0; |
|
| 158 | 176 |
return; |
| 159 | 177 |
} |
| 160 | 178 |
|
| ... | ... | |
| 171 | 189 |
switch(error) |
| 172 | 190 |
{
|
| 173 | 191 |
case 0: |
| 174 |
*turn_speed = 0;
|
|
| 175 |
*speed = SPEED;
|
|
| 192 |
turn_speed = 0; |
|
| 193 |
speed = SPEED; |
|
| 176 | 194 |
return; |
| 177 | 195 |
case 1: |
| 178 | 196 |
case -1: |
| 179 |
*turn_speed = error * 5;
|
|
| 180 |
*speed = SPEED;
|
|
| 197 |
turn_speed = error * 5; |
|
| 198 |
speed = SPEED; |
|
| 181 | 199 |
return; |
| 182 | 200 |
case 2: |
| 183 | 201 |
case -2: |
| 184 |
*turn_speed = error * 8;
|
|
| 185 |
*speed = SPEED;
|
|
| 202 |
turn_speed = error * 8; |
|
| 203 |
speed = SPEED; |
|
| 186 | 204 |
return; |
| 187 | 205 |
case 3: |
| 188 | 206 |
case -3: |
| 189 |
*turn_speed = error * 12;
|
|
| 190 |
*speed = SPEED;
|
|
| 207 |
turn_speed = error * 12; |
|
| 208 |
speed = SPEED; |
|
| 191 | 209 |
return; |
| 192 | 210 |
case 4: |
| 193 | 211 |
case -4: |
| 194 |
*turn_speed = error * 16;
|
|
| 195 |
*speed = SPEED - SPEED * error * error / 36;
|
|
| 212 |
turn_speed = error * 16; |
|
| 213 |
speed = SPEED - SPEED * error * error / 36; |
|
| 196 | 214 |
return; |
| 197 | 215 |
} |
| 198 | 216 |
// if not in a case |
| 199 |
*turn_speed = error * 20;
|
|
| 200 |
*speed = SPEED - SPEED * error * error / 36;
|
|
| 217 |
turn_speed = error * 20; |
|
| 218 |
speed = SPEED - SPEED * error * error / 36; |
|
| 201 | 219 |
} |
| 202 | 220 |
|
| 203 | 221 |
// create connected components and pick a leader for each chain |
Also available in: Unified diff