Revision 258
cleaned up lemmings code
added more comments
branches/lemmings/code/behaviors/lemmings/driver.c | ||
---|---|---|
7 | 7 |
#include <wl_token_ring.h> |
8 | 8 |
#include "lemmings.h" |
9 | 9 |
|
10 |
/* ORB COLORS |
|
11 |
RED: light blue |
|
12 |
ORANGE: light blue |
|
13 |
YELLOW: light blue |
|
14 |
GREEN: |
|
15 |
BLUE: |
|
16 |
*/ |
|
17 | 10 |
|
18 | 11 |
int main(void) { |
12 |
// enable everything |
|
19 | 13 |
dragonfly_init(ALL_ON); |
20 | 14 |
orb_enable(); |
21 |
|
|
22 | 15 |
wl_init(); |
23 | 16 |
wl_token_ring_register(); |
24 | 17 |
wl_token_ring_join(); // join token ring |
... | ... | |
27 | 20 |
|
28 | 21 |
while(1) { |
29 | 22 |
wl_do(); |
30 |
lemmings_FSM(); |
|
23 |
lemmings_FSM(); // do lemmings
|
|
31 | 24 |
} |
32 | 25 |
|
33 | 26 |
return 0; |
branches/lemmings/code/behaviors/lemmings/lemmings.c | ||
---|---|---|
3 | 3 |
#include <wireless.h> |
4 | 4 |
#include <wl_token_ring.h> |
5 | 5 |
//#include <colonet_dragonfly.h> |
6 |
#include "smart_run_around_fsm.h" |
|
6 |
//#include "smart_run_around_fsm.h"
|
|
7 | 7 |
#include "lemmings.h" |
8 | 8 |
|
9 |
/*A simple behavior for following the leader.
|
|
9 |
/*A simple behavior for following the leader |
|
10 | 10 |
SINGLE_FILE pattern not implemented yet |
11 | 11 |
*/ |
12 | 12 |
|
13 | 13 |
// FSM states |
14 |
#define LEAD 0 // move around randomly, leading a chain
|
|
15 |
#define FOLLOW 1 // follow another bot |
|
14 |
#define LEAD 0 // be a leader
|
|
15 |
#define FOLLOW 1 // follow another robot
|
|
16 | 16 |
|
17 | 17 |
#define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern |
18 |
#define SPEED 200 |
|
18 |
#define SPEED 200 // set default speed
|
|
19 | 19 |
|
20 | 20 |
/* Globals */ |
21 | 21 |
static int cur_state; // current state |
22 |
static int local_leader; // follow this bot (-1 if leader) |
|
23 |
static int global_leader; |
|
24 |
static int follow_multi; // set to 0 for single-file following, 1 for tree following |
|
25 |
static int local_id; |
|
26 |
static int num_robots; |
|
22 |
static int local_leader; // follow this bot (-1 if leader) |
|
23 |
static int global_leader; // this is the global leader |
|
24 |
static int follow_multi; // set to 0 for single-file following, 1 for tree following |
|
25 |
static int local_id; // this is the local robot id |
|
27 | 26 |
|
28 | 27 |
/* Internal prototypes */ |
29 | 28 |
static int get_local_leader(void); |
... | ... | |
33 | 32 |
void lemmings_init() { |
34 | 33 |
// usb_puts("lemmings_init\n"); |
35 | 34 |
|
36 |
// colonet_init(); |
|
37 |
//run_around_init(); // prepare for moving
|
|
35 |
// colonet_init();
|
|
36 |
//run_around_init(); // prepare for moving |
|
38 | 37 |
|
38 |
// set to defaults |
|
39 | 39 |
cur_state = FOLLOW; |
40 | 40 |
local_leader = -1; |
41 | 41 |
global_leader = -1; |
42 | 42 |
local_id = wl_get_xbee_id(); |
43 |
num_robots = 0; |
|
44 | 43 |
|
45 | 44 |
follow_multi = FOLLOW_MULTI_DEFAULT; |
46 | 45 |
} |
... | ... | |
52 | 51 |
|
53 | 52 |
|
54 | 53 |
if (wl_token_get_num_robots() <= 1) { |
54 |
/* if there is only one robot in the token ring, there is nothing to |
|
55 |
follow, so don't do anything */ |
|
55 | 56 |
motors_off(); |
56 | 57 |
return; |
57 | 58 |
} |
... | ... | |
60 | 61 |
//usb_puti(cur_state); |
61 | 62 |
//usb_puts("\n"); |
62 | 63 |
|
63 |
// Elect a leader if number of robots changes -- max id robot. |
|
64 |
/* usb_puts("num robots: "); */ |
|
65 |
/* usb_puti(num_robots); */ |
|
66 |
/* usb_puts("\nnew num robots: "); */ |
|
67 |
/* usb_puti(new_num_robots); */ |
|
68 |
/* usb_puts("\n"); */ |
|
69 |
|
|
64 |
// only check for new global leader once every 5 iterations |
|
70 | 65 |
leader_check_count++; |
71 | 66 |
if (leader_check_count % 5 == 0) { |
72 | 67 |
wl_token_iterator_begin(); |
73 | 68 |
|
69 |
// find robot with max id and assign as leader |
|
74 | 70 |
int max = -1; |
75 | 71 |
while (wl_token_iterator_has_next()) { |
76 | 72 |
int n = wl_token_iterator_next(); |
... | ... | |
82 | 78 |
} |
83 | 79 |
|
84 | 80 |
/* usb_puti(local_id); */ |
85 |
/* usb_puts(" is the local \n"); */ |
|
81 |
/* usb_puts(" is the local robot\n"); */
|
|
86 | 82 |
|
87 | 83 |
if (max == local_id) { |
84 |
// become the global leader |
|
88 | 85 |
orb_set_color(BLUE); |
89 | 86 |
cur_state = LEAD; |
87 |
local_leader = -1; |
|
90 | 88 |
// usb_puts(" becoming leader \n"); |
91 | 89 |
} else { |
90 |
// become a follower |
|
92 | 91 |
orb_set_color(RED); |
93 | 92 |
cur_state = FOLLOW; |
94 | 93 |
// usb_puts(" becoming follower\n"); |
95 | 94 |
} |
96 | 95 |
|
97 |
global_leader = max; |
|
96 |
global_leader = max; // set the global leader
|
|
98 | 97 |
} |
99 | 98 |
|
100 | 99 |
switch (cur_state) { |
101 | 100 |
case LEAD: |
101 |
// do lead action - move straight ahead |
|
102 | 102 |
orb_set_color(BLUE); |
103 |
|
|
104 |
//if (local_leader != -1) { |
|
105 |
//usb_puts("warning: in LEAD state, but local_leader != -1.\n"); |
|
106 |
// } |
|
107 |
|
|
108 | 103 |
move(SPEED, 0); |
109 |
// motors_off(); |
|
110 |
// run_around_FSM(); |
|
111 | 104 |
break; |
112 | 105 |
|
113 | 106 |
case FOLLOW: |
114 | 107 |
orb_set_color(RED); |
115 | 108 |
|
116 |
// Make sure we can see our local leader.
|
|
117 |
leader_bom = wl_token_get_sensor_reading(local_id, local_leader);
|
|
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;
|
|
118 | 111 |
|
119 | 112 |
if (leader_bom < 0) { |
120 | 113 |
//usb_puts("no connection to leader\n"); |
... | ... | |
123 | 116 |
local_leader = get_local_leader(); |
124 | 117 |
|
125 | 118 |
if (local_leader == -1) { |
119 |
// nothing to follow, so become a local_leader and start leading |
|
126 | 120 |
//usb_puts("can't see anyone - becoming a leader\n"); |
127 | 121 |
cur_state = LEAD; |
122 |
local_leader = -1; |
|
123 |
orb_set_color(BLUE); |
|
124 |
move(SPEED, 0); |
|
125 |
return; |
|
128 | 126 |
} else { |
129 | 127 |
//usb_puts("new leader is"); |
130 | 128 |
//usb_puti(local_leader); |
131 | 129 |
//usb_puts("\n"); |
132 |
|
|
130 |
// get data in order to follow leader |
|
133 | 131 |
leader_bom = wl_token_get_sensor_reading(local_id, local_leader); |
134 | 132 |
get_bom_velocity(leader_bom, &turn_speed, &speed); |
135 | 133 |
} |
136 | 134 |
} else { |
137 |
// Leader is still in sight.
|
|
135 |
// Leader is still in sight, so follow it
|
|
138 | 136 |
get_bom_velocity(leader_bom, &turn_speed, &speed); |
139 | 137 |
} |
140 |
|
|
141 |
move(speed, turn_speed); // move in direction of bot |
|
142 |
break; |
|
143 |
|
|
144 |
// default: |
|
145 |
// usb_puts("ERROR - invalid state."); |
|
146 |
} |
|
147 |
|
|
138 |
|
|
148 | 139 |
//usb_puts("following robot# "); |
149 | 140 |
//usb_puti(local_leader); |
150 | 141 |
//usb_puts(" bom of leader: "); |
151 | 142 |
//usb_puti(leader_bom); |
152 | 143 |
//usb_puts("\n"); |
144 |
|
|
145 |
move(speed, turn_speed); // move in direction of leader |
|
146 |
break; |
|
147 |
} |
|
153 | 148 |
} |
154 | 149 |
|
150 |
// translate bom reading into direction and speed |
|
155 | 151 |
static void get_bom_velocity(int bom_id, int* turn_speed, int* speed) { |
156 | 152 |
int error = 0; |
157 | 153 |
|
... | ... | |
162 | 158 |
return; |
163 | 159 |
} |
164 | 160 |
|
161 |
// set so right is negative, left is positive |
|
165 | 162 |
if (bom_id <= 12) { |
166 | 163 |
error = bom_id - 4; |
167 | 164 |
} else { |
168 | 165 |
error = bom_id - 20; |
169 | 166 |
} |
170 | 167 |
|
171 |
*turn_speed = error * 20; |
|
172 |
*speed = -SPEED * error * error / 25 + SPEED; |
|
168 |
*turn_speed = error * 20; // set turn speed
|
|
169 |
*speed = -SPEED * error * error / 25 + SPEED; // set speed
|
|
173 | 170 |
} |
174 | 171 |
|
175 | 172 |
// create connected components and pick a leader for each chain |
176 |
// use modified Prim's alogrithm to find local spanning tree |
|
177 | 173 |
// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW |
178 | 174 |
static int get_local_leader(void) { |
179 | 175 |
int nodeB; |
... | ... | |
183 | 179 |
|
184 | 180 |
wl_token_iterator_begin(); |
185 | 181 |
|
182 |
// iterator through robots in token ring |
|
186 | 183 |
while (wl_token_iterator_has_next()) { |
187 | 184 |
nodeB = wl_token_iterator_next(); |
188 | 185 |
if (nodeB == local_id) { |
189 | 186 |
continue; // can't follow self |
190 | 187 |
} |
191 | 188 |
|
189 |
// get an edge weight based on the robot position |
|
192 | 190 |
cur_weight = get_edge_weight(local_id, nodeB); |
193 | 191 |
|
194 | 192 |
if (nodeB == global_leader && cur_weight != -1) { |
193 |
// always follow the global leader if you have the option to |
|
195 | 194 |
best_node = nodeB; |
196 | 195 |
break; |
197 | 196 |
} |
Also available in: Unified diff