root / branches / lemmings / code / behaviors / lemmings / lemmings.c @ 258
History | View | Annotate | Download (5.94 KB)
1 | 209 | dsschult | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wl_defs.h> |
||
3 | #include <wireless.h> |
||
4 | #include <wl_token_ring.h> |
||
5 | 243 | emarinel | //#include <colonet_dragonfly.h>
|
6 | 258 | dsschult | //#include "smart_run_around_fsm.h"
|
7 | 209 | dsschult | #include "lemmings.h" |
8 | |||
9 | 258 | dsschult | /*A simple behavior for following the leader
|
10 | 209 | dsschult | SINGLE_FILE pattern not implemented yet
|
11 | */
|
||
12 | |||
13 | 233 | emarinel | // FSM states
|
14 | 258 | dsschult | #define LEAD 0 // be a leader |
15 | #define FOLLOW 1 // follow another robot |
||
16 | 209 | dsschult | |
17 | 230 | emarinel | #define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern |
18 | 258 | dsschult | #define SPEED 200 // set default speed |
19 | 230 | emarinel | |
20 | /* Globals */
|
||
21 | 239 | emarinel | static int cur_state; // current state |
22 | 258 | dsschult | 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 |
||
26 | 230 | emarinel | |
27 | 233 | emarinel | /* Internal prototypes */
|
28 | static int get_local_leader(void); |
||
29 | static int get_edge_weight(int robot1, int robot2); |
||
30 | 239 | emarinel | static void get_bom_velocity(int bom_id, int* turn_speed, int* speed); |
31 | 230 | emarinel | |
32 | void lemmings_init() {
|
||
33 | 239 | emarinel | // usb_puts("lemmings_init\n");
|
34 | 230 | emarinel | |
35 | 258 | dsschult | // colonet_init();
|
36 | //run_around_init(); // prepare for moving
|
||
37 | 230 | emarinel | |
38 | 258 | dsschult | // set to defaults
|
39 | 243 | emarinel | cur_state = FOLLOW; |
40 | 233 | emarinel | local_leader = -1;
|
41 | 243 | emarinel | global_leader = -1;
|
42 | 233 | emarinel | local_id = wl_get_xbee_id(); |
43 | 230 | emarinel | |
44 | 233 | emarinel | follow_multi = FOLLOW_MULTI_DEFAULT; |
45 | } |
||
46 | 230 | emarinel | |
47 | 233 | emarinel | /* The main function, call this to update states as frequently as possible. */
|
48 | void lemmings_FSM(void) { |
||
49 | 239 | emarinel | int leader_bom = 0, turn_speed = 0, speed = 0; |
50 | 243 | emarinel | static int leader_check_count = 0; |
51 | 230 | emarinel | |
52 | 243 | emarinel | |
53 | if (wl_token_get_num_robots() <= 1) { |
||
54 | 258 | dsschult | /* if there is only one robot in the token ring, there is nothing to
|
55 | follow, so don't do anything */
|
||
56 | 243 | emarinel | motors_off(); |
57 | return;
|
||
58 | } |
||
59 | |||
60 | 239 | emarinel | //usb_puts("state: ");
|
61 | //usb_puti(cur_state);
|
||
62 | //usb_puts("\n");
|
||
63 | 209 | dsschult | |
64 | 258 | dsschult | // only check for new global leader once every 5 iterations
|
65 | 243 | emarinel | leader_check_count++; |
66 | if (leader_check_count % 5 == 0) { |
||
67 | 239 | emarinel | wl_token_iterator_begin(); |
68 | 243 | emarinel | |
69 | 258 | dsschult | // find robot with max id and assign as leader
|
70 | 239 | emarinel | int max = -1; |
71 | while (wl_token_iterator_has_next()) {
|
||
72 | int n = wl_token_iterator_next();
|
||
73 | if (n > max) {
|
||
74 | max = n; |
||
75 | } |
||
76 | 243 | emarinel | /* usb_puti(n); */
|
77 | /* usb_puts("\n"); */
|
||
78 | 233 | emarinel | } |
79 | 243 | emarinel | |
80 | /* usb_puti(local_id); */
|
||
81 | 258 | dsschult | /* usb_puts(" is the local robot\n"); */
|
82 | 209 | dsschult | |
83 | 239 | emarinel | if (max == local_id) {
|
84 | 258 | dsschult | // become the global leader
|
85 | 243 | emarinel | orb_set_color(BLUE); |
86 | 239 | emarinel | cur_state = LEAD; |
87 | 258 | dsschult | local_leader = -1;
|
88 | 243 | emarinel | // usb_puts(" becoming leader \n");
|
89 | 239 | emarinel | } else {
|
90 | 258 | dsschult | // become a follower
|
91 | 243 | emarinel | orb_set_color(RED); |
92 | 233 | emarinel | cur_state = FOLLOW; |
93 | 243 | emarinel | // usb_puts(" becoming follower\n");
|
94 | 233 | emarinel | } |
95 | 230 | emarinel | |
96 | 258 | dsschult | global_leader = max; // set the global leader
|
97 | 239 | emarinel | } |
98 | |||
99 | switch (cur_state) {
|
||
100 | case LEAD:
|
||
101 | 258 | dsschult | // do lead action - move straight ahead
|
102 | 239 | emarinel | orb_set_color(BLUE); |
103 | move(SPEED, 0);
|
||
104 | 233 | emarinel | break;
|
105 | 230 | emarinel | |
106 | 233 | emarinel | case FOLLOW:
|
107 | 239 | emarinel | orb_set_color(RED); |
108 | 230 | emarinel | |
109 | 258 | dsschult | // 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 | 233 | emarinel | |
112 | if (leader_bom < 0) { |
||
113 | 239 | emarinel | //usb_puts("no connection to leader\n");
|
114 | |||
115 | 233 | emarinel | // Can't see our old leader -- get new bot to follow.
|
116 | local_leader = get_local_leader(); |
||
117 | |||
118 | if (local_leader == -1) { |
||
119 | 258 | dsschult | // nothing to follow, so become a local_leader and start leading
|
120 | 239 | emarinel | //usb_puts("can't see anyone - becoming a leader\n");
|
121 | 233 | emarinel | cur_state = LEAD; |
122 | 258 | dsschult | local_leader = -1;
|
123 | orb_set_color(BLUE); |
||
124 | move(SPEED, 0);
|
||
125 | return;
|
||
126 | 239 | emarinel | } else {
|
127 | //usb_puts("new leader is");
|
||
128 | //usb_puti(local_leader);
|
||
129 | //usb_puts("\n");
|
||
130 | 258 | dsschult | // get data in order to follow leader
|
131 | 239 | emarinel | leader_bom = wl_token_get_sensor_reading(local_id, local_leader); |
132 | get_bom_velocity(leader_bom, &turn_speed, &speed); |
||
133 | 209 | dsschult | } |
134 | 233 | emarinel | } else {
|
135 | 258 | dsschult | // Leader is still in sight, so follow it
|
136 | 239 | emarinel | get_bom_velocity(leader_bom, &turn_speed, &speed); |
137 | 209 | dsschult | } |
138 | 258 | dsschult | |
139 | 239 | emarinel | //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 | 258 | dsschult | |
145 | move(speed, turn_speed); // move in direction of leader
|
||
146 | break;
|
||
147 | } |
||
148 | 233 | emarinel | } |
149 | 230 | emarinel | |
150 | 258 | dsschult | // translate bom reading into direction and speed
|
151 | 239 | emarinel | static void get_bom_velocity(int bom_id, int* turn_speed, int* speed) { |
152 | int error = 0; |
||
153 | 233 | emarinel | |
154 | 239 | emarinel | if (bom_id < 0 || bom_id > 15) { |
155 | // usb_puts("ERROR - invalid bom_id\n");
|
||
156 | *turn_speed = 0;
|
||
157 | *speed = 0;
|
||
158 | 243 | emarinel | return;
|
159 | 239 | emarinel | } |
160 | 233 | emarinel | |
161 | 258 | dsschult | // set so right is negative, left is positive
|
162 | 239 | emarinel | if (bom_id <= 12) { |
163 | error = bom_id - 4;
|
||
164 | } else {
|
||
165 | error = bom_id - 20;
|
||
166 | 233 | emarinel | } |
167 | |||
168 | 258 | dsschult | *turn_speed = error * 20; // set turn speed |
169 | *speed = -SPEED * error * error / 25 + SPEED; // set speed |
||
170 | 209 | dsschult | } |
171 | |||
172 | // create connected components and pick a leader for each chain
|
||
173 | // CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
|
||
174 | 233 | emarinel | static int get_local_leader(void) { |
175 | int nodeB;
|
||
176 | int cur_weight;
|
||
177 | 209 | dsschult | int best_weight = 1000; // set to infinity initially |
178 | int best_node = -1; |
||
179 | 230 | emarinel | |
180 | 209 | dsschult | wl_token_iterator_begin(); |
181 | 230 | emarinel | |
182 | 258 | dsschult | // iterator through robots in token ring
|
183 | 233 | emarinel | while (wl_token_iterator_has_next()) {
|
184 | 209 | dsschult | nodeB = wl_token_iterator_next(); |
185 | 233 | emarinel | if (nodeB == local_id) {
|
186 | 209 | dsschult | continue; // can't follow self |
187 | 230 | emarinel | } |
188 | |||
189 | 258 | dsschult | // get an edge weight based on the robot position
|
190 | 233 | emarinel | cur_weight = get_edge_weight(local_id, nodeB); |
191 | 230 | emarinel | |
192 | 243 | emarinel | if (nodeB == global_leader && cur_weight != -1) { |
193 | 258 | dsschult | // always follow the global leader if you have the option to
|
194 | 243 | emarinel | best_node = nodeB; |
195 | break;
|
||
196 | } |
||
197 | |||
198 | 212 | dsschult | if (cur_weight >= 0 && cur_weight < best_weight) { |
199 | 233 | emarinel | // this is new best node, so save values.
|
200 | 209 | dsschult | best_weight = cur_weight; |
201 | best_node = nodeB; |
||
202 | } |
||
203 | } |
||
204 | 230 | emarinel | |
205 | 233 | emarinel | return best_node;
|
206 | 209 | dsschult | } |
207 | |||
208 | // get edge weight of sensor matrix
|
||
209 | // add in BOM range data when BOM 1.5 comes out
|
||
210 | 233 | emarinel | static int get_edge_weight(int robot1, int robot2) { |
211 | 209 | dsschult | int bom = wl_token_get_sensor_reading(robot1,robot2);
|
212 | 233 | emarinel | |
213 | // Robots closer to directly in front of us have lower weight.
|
||
214 | 209 | dsschult | switch(bom) {
|
215 | case 12: |
||
216 | 233 | emarinel | return 10; |
217 | |||
218 | 209 | dsschult | case 13: |
219 | case 11: |
||
220 | 233 | emarinel | return 9; |
221 | |||
222 | 209 | dsschult | case 14: |
223 | case 10: |
||
224 | 233 | emarinel | return 8; |
225 | |||
226 | 209 | dsschult | case 15: |
227 | case 9: |
||
228 | 233 | emarinel | return 7; |
229 | |||
230 | 209 | dsschult | case 0: |
231 | case 8: |
||
232 | 233 | emarinel | return 6; |
233 | |||
234 | 209 | dsschult | case 1: |
235 | case 7: |
||
236 | 233 | emarinel | return 5; |
237 | |||
238 | 209 | dsschult | case 2: |
239 | case 6: |
||
240 | 233 | emarinel | return 4; |
241 | |||
242 | 209 | dsschult | case 3: |
243 | case 5: |
||
244 | 233 | emarinel | return 3; |
245 | |||
246 | 209 | dsschult | case 4: |
247 | 233 | emarinel | return 2; |
248 | 209 | dsschult | } |
249 | |||
250 | 233 | emarinel | return -1; |
251 | 209 | dsschult | } |