root / branches / lemmings / code / behaviors / lemmings / lemmings.c @ 233
History | View | Annotate | Download (4.08 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wl_defs.h> |
3 |
#include <wireless.h> |
4 |
#include <wl_token_ring.h> |
5 |
#include "smart_run_around_fsm.h" |
6 |
#include "lemmings.h" |
7 |
|
8 |
/*A simple behavior for following the leader.
|
9 |
SINGLE_FILE pattern not implemented yet
|
10 |
*/
|
11 |
|
12 |
// FSM states
|
13 |
#define LEAD 0 // move around randomly, leading a chain |
14 |
#define FOLLOW 1 // follow another bot |
15 |
|
16 |
#define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern |
17 |
#define CRAZY_MAX 200 // The number of counts between "crazy moments" |
18 |
#define SPEED 200 |
19 |
|
20 |
/* Globals */
|
21 |
int cur_state; // current state |
22 |
int crazy_count; // Counter for a random behavior |
23 |
int local_leader; // follow this bot (-1 if leader) |
24 |
int follow_multi; // set to 0 for single-file following, 1 for tree following |
25 |
int local_id;
|
26 |
|
27 |
/* Internal prototypes */
|
28 |
static int get_local_leader(void); |
29 |
static int get_edge_weight(int robot1, int robot2); |
30 |
static int get_bom_angle(int bom_id); |
31 |
|
32 |
void lemmings_init() {
|
33 |
usb_puts("lemmings_init\n");
|
34 |
|
35 |
run_around_init(); // prepare for moving
|
36 |
|
37 |
cur_state = LEAD; |
38 |
crazy_count = CRAZY_MAX; |
39 |
local_leader = -1;
|
40 |
local_id = wl_get_xbee_id(); |
41 |
|
42 |
follow_multi = FOLLOW_MULTI_DEFAULT; |
43 |
} |
44 |
|
45 |
/* The main function, call this to update states as frequently as possible. */
|
46 |
void lemmings_FSM(void) { |
47 |
int leader_bom = -1, old_follow_bot; |
48 |
int follow_direction = 0; |
49 |
|
50 |
usb_puts("state: ");
|
51 |
usb_puti(cur_state); |
52 |
usb_puts("\n\r");
|
53 |
|
54 |
switch (cur_state) {
|
55 |
case LEAD:
|
56 |
orb_set_color(WHITE); |
57 |
|
58 |
if (local_leader != -1) { |
59 |
usb_puts("warning: in LEAD state, but local_leader != -1.\n");
|
60 |
} |
61 |
|
62 |
local_leader = get_local_leader(); |
63 |
if (local_leader != -1) { |
64 |
cur_state = FOLLOW; |
65 |
} |
66 |
|
67 |
run_around_FSM(); |
68 |
break;
|
69 |
|
70 |
case FOLLOW:
|
71 |
old_follow_bot = local_leader; |
72 |
|
73 |
// Make sure we can see our local leader.
|
74 |
leader_bom = wl_token_get_sensor_reading(local_id, local_leader); |
75 |
|
76 |
if (leader_bom < 0) { |
77 |
// Can't see our old leader -- get new bot to follow.
|
78 |
local_leader = get_local_leader(); |
79 |
|
80 |
if (local_leader == -1) { |
81 |
cur_state = LEAD; |
82 |
} |
83 |
} else {
|
84 |
// Leader is still in sight.
|
85 |
follow_direction = get_bom_angle(leader_bom); |
86 |
} |
87 |
|
88 |
default:
|
89 |
usb_puts("ERROR - invalid state.");
|
90 |
} |
91 |
|
92 |
move(SPEED, follow_direction); // move in direction of bot
|
93 |
|
94 |
usb_puts("following robot# ");
|
95 |
usb_puti(local_leader); |
96 |
usb_puts(" bom of leader: ");
|
97 |
usb_puti(leader_bom); |
98 |
usb_puts("\n\r");
|
99 |
} |
100 |
|
101 |
static int get_bom_angle(int bom_id) { |
102 |
switch (bom_id) {
|
103 |
case 0: |
104 |
case 1: |
105 |
return 150; |
106 |
|
107 |
case 2: |
108 |
return 100; |
109 |
|
110 |
case 3: |
111 |
return 50; |
112 |
|
113 |
case 4: |
114 |
return 0; |
115 |
|
116 |
case 5: |
117 |
return -50; |
118 |
|
119 |
case 6: |
120 |
return -100; |
121 |
|
122 |
case 7: |
123 |
return -150; |
124 |
|
125 |
case 8: |
126 |
case 9: |
127 |
case 10: |
128 |
case 11: |
129 |
return -255; |
130 |
|
131 |
case 12: |
132 |
case 13: |
133 |
case 14: |
134 |
case 15: |
135 |
return 255; |
136 |
} |
137 |
|
138 |
return 0; |
139 |
} |
140 |
|
141 |
// create connected components and pick a leader for each chain
|
142 |
// use modified Prim's alogrithm to find local spanning tree
|
143 |
// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
|
144 |
static int get_local_leader(void) { |
145 |
int nodeB;
|
146 |
int cur_weight;
|
147 |
int best_weight = 1000; // set to infinity initially |
148 |
int best_node = -1; |
149 |
|
150 |
wl_token_iterator_begin(); |
151 |
|
152 |
while (wl_token_iterator_has_next()) {
|
153 |
nodeB = wl_token_iterator_next(); |
154 |
if (nodeB == local_id) {
|
155 |
continue; // can't follow self |
156 |
} |
157 |
|
158 |
cur_weight = get_edge_weight(local_id, nodeB); |
159 |
|
160 |
if (cur_weight >= 0 && cur_weight < best_weight) { |
161 |
// this is new best node, so save values.
|
162 |
best_weight = cur_weight; |
163 |
best_node = nodeB; |
164 |
} |
165 |
} |
166 |
|
167 |
return best_node;
|
168 |
} |
169 |
|
170 |
// get edge weight of sensor matrix
|
171 |
// add in BOM range data when BOM 1.5 comes out
|
172 |
static int get_edge_weight(int robot1, int robot2) { |
173 |
int bom = wl_token_get_sensor_reading(robot1,robot2);
|
174 |
|
175 |
// Robots closer to directly in front of us have lower weight.
|
176 |
switch(bom) {
|
177 |
case 12: |
178 |
return 10; |
179 |
|
180 |
case 13: |
181 |
case 11: |
182 |
return 9; |
183 |
|
184 |
case 14: |
185 |
case 10: |
186 |
return 8; |
187 |
|
188 |
case 15: |
189 |
case 9: |
190 |
return 7; |
191 |
|
192 |
case 0: |
193 |
case 8: |
194 |
return 6; |
195 |
|
196 |
case 1: |
197 |
case 7: |
198 |
return 5; |
199 |
|
200 |
case 2: |
201 |
case 6: |
202 |
return 4; |
203 |
|
204 |
case 3: |
205 |
case 5: |
206 |
return 3; |
207 |
|
208 |
case 4: |
209 |
return 2; |
210 |
} |
211 |
|
212 |
return -1; |
213 |
} |