Revision 1500
Commented the template file and my implementation of Hunter-Prey to make the code easier to follow.
main.c | ||
---|---|---|
1 |
/* |
|
2 |
* Hunter-Prey main.c File - Implementation of Hunter-Prey behavior which |
|
3 |
* uses finite state machines to manage the behavior. A top level |
|
4 |
* state machine controls the high level behavior switches between |
|
5 |
* "hunter" and "prey" and manages the wireless communication. Two |
|
6 |
* additional state machines control the behavior of the robot when |
|
7 |
* it is in "prey" mode and when it is in "hunter" mode. |
|
8 |
* |
|
9 |
* Author: John Sexton, Colony Project, CMU Robotics Club |
|
10 |
*/ |
|
11 |
|
|
1 | 12 |
#include <dragonfly_lib.h> |
2 | 13 |
#include <wl_basic.h> |
3 | 14 |
#include "hunter_prey.h" |
... | ... | |
11 | 22 |
|
12 | 23 |
/* State Macros */ |
13 | 24 |
|
14 |
/* Top Level FSM */ |
|
25 |
/* Top Level FSM States */
|
|
15 | 26 |
#define TOP_INIT 0 |
16 | 27 |
#define TOP_HUNTER_HUNT 1 |
17 | 28 |
#define TOP_HUNTER_TAG 2 |
... | ... | |
20 | 31 |
#define TOP_HUNTER_WAIT 5 |
21 | 32 |
#define TOP_ERROR 6 |
22 | 33 |
|
23 |
/* Hunter FSM */ |
|
34 |
/* Hunter FSM States */
|
|
24 | 35 |
#define HUNTER_SPIRAL 0 |
25 | 36 |
#define HUNTER_CHASE 1 |
26 | 37 |
|
27 |
/* Prey FSM */ |
|
38 |
/* Prey FSM States */
|
|
28 | 39 |
#define PREY_START_BACK 0 |
29 | 40 |
#define PREY_BACKING 1 |
30 | 41 |
#define PREY_TURN 2 |
... | ... | |
71 | 82 |
|
72 | 83 |
while (1) { |
73 | 84 |
|
85 |
/* Check if we've received a wireless packet */ |
|
74 | 86 |
packet_data = wl_basic_do_default(&data_length); |
75 | 87 |
|
88 |
/* Top level state machines */ |
|
76 | 89 |
switch(state) { |
77 | 90 |
|
78 | 91 |
case TOP_INIT: |
... | ... | |
81 | 94 |
orbs_set_color(GREEN, RED); |
82 | 95 |
delay_ms(500); |
83 | 96 |
|
97 |
/* Allow user to pick the starting behavior */ |
|
84 | 98 |
if (button1_read()) { |
85 | 99 |
state = TOP_PREY_AVOID; |
86 | 100 |
prey_state = PREY_AVOID; |
... | ... | |
94 | 108 |
|
95 | 109 |
if (packet_data && data_length == 2 |
96 | 110 |
&& packet_data[0] == HUNTER_PREY_ACTION_ACK) { |
111 |
/* If we've received an ACK, we need to wait */ |
|
97 | 112 |
state = TOP_HUNTER_WAIT; |
98 | 113 |
} else { |
114 |
/* Record some sensor readings and check if we can TAG */ |
|
99 | 115 |
bom_refresh(BOM_ALL); |
100 | 116 |
frontIR = range_read_distance(IR2); |
101 | 117 |
maxBOM = get_max_bom(); |
102 | 118 |
if (hunter_prey_tagged(maxBOM, frontIR) || button1_read()) { |
103 | 119 |
state = TOP_HUNTER_TAG; |
104 | 120 |
} else { |
121 |
/* If we haven't tagged, then enter hunter FSM */ |
|
105 | 122 |
hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR); |
106 | 123 |
} |
107 | 124 |
} |
... | ... | |
112 | 129 |
|
113 | 130 |
if (packet_data && data_length == 2 |
114 | 131 |
&& packet_data[0] == HUNTER_PREY_ACTION_ACK) { |
132 |
/* If we've received an ACK, then someone beat us to the TAG and |
|
133 |
* we need to wait. */ |
|
115 | 134 |
state = TOP_HUNTER_WAIT; |
116 | 135 |
} else { |
136 |
/* Prepare and send the TAG packet */ |
|
117 | 137 |
send_buffer[0] = HUNTER_PREY_ACTION_TAG; |
118 | 138 |
send_buffer[1] = robotID; |
119 | 139 |
wl_basic_send_global_packet(42, send_buffer, 2); |
120 | 140 |
|
141 |
/* Record the time so we don't spam a TAG message on the network */ |
|
121 | 142 |
oldTime = rtc_get(); |
122 | 143 |
state = TOP_HUNTER_PURSUE; |
123 | 144 |
} |
... | ... | |
128 | 149 |
|
129 | 150 |
if (packet_data && data_length == 2 |
130 | 151 |
&& packet_data[0] == HUNTER_PREY_ACTION_ACK) { |
152 |
/* Check if we've received a new wireless packet */ |
|
131 | 153 |
|
132 | 154 |
if (packet_data[1] == robotID) { |
155 |
/* We've been ACKed, so we can now become the prey */ |
|
133 | 156 |
state = TOP_PREY_AVOID; |
134 | 157 |
prey_state = PREY_START_BACK; |
135 | 158 |
} else { |
159 |
/* If we get an ACK with a different robotID, then someone beat us |
|
160 |
* to the TAG, so we must wait */ |
|
136 | 161 |
state = TOP_HUNTER_WAIT; |
137 | 162 |
} |
138 | 163 |
|
139 | 164 |
} else if (curTime - oldTime > 1) { |
165 |
/* If 1 second has ellapsed, return to normal hunting state (we can |
|
166 |
* TAG again now) */ |
|
140 | 167 |
state = TOP_HUNTER_HUNT; |
141 |
} else if (oldTime > curTime) { // If overflow or was reset, restart |
|
168 |
} else if (oldTime > curTime) { |
|
169 |
/* If for some reason the timer overflows, or the wireless library |
|
170 |
* (which is also using the same timer) resets the timer, |
|
171 |
* reinitialize the timer so that we don't wait too long for the |
|
172 |
* timer to catch back up. */ |
|
142 | 173 |
oldTime = curTime; |
143 | 174 |
} else { |
175 |
/* If no other behavioral changes need to be made, then continue |
|
176 |
* with the hunter FSM where we left off */ |
|
144 | 177 |
bom_refresh(BOM_ALL); |
145 | 178 |
frontIR = range_read_distance(IR2); |
146 | 179 |
maxBOM = get_max_bom(); |
... | ... | |
151 | 184 |
orbs_set_color(GREEN, GREEN); |
152 | 185 |
if (packet_data && data_length == 2 |
153 | 186 |
&& packet_data[0] == HUNTER_PREY_ACTION_TAG) { |
187 |
/* Check if we've received a TAG yet. If so then send an ACK back */ |
|
154 | 188 |
|
155 | 189 |
send_buffer[0] = HUNTER_PREY_ACTION_ACK; |
156 | 190 |
send_buffer[1] = packet_data[1]; |
... | ... | |
160 | 194 |
motors_off(); |
161 | 195 |
state = TOP_HUNTER_WAIT; |
162 | 196 |
} else { |
197 |
/* If we haven't received a TAG yet, continue with prey FSM */ |
|
163 | 198 |
bom_on(); |
164 | 199 |
prey_state = prey_FSM(prey_state); |
165 | 200 |
} |
166 | 201 |
break; |
167 | 202 |
case TOP_HUNTER_WAIT: |
203 |
/* Set orb colors and wait to give the prey the 5 second head start */ |
|
168 | 204 |
orbs_set_color(BLUE, BLUE); |
169 | 205 |
delay_ms(5000); |
170 | 206 |
state = TOP_HUNTER_HUNT; |
... | ... | |
188 | 224 |
|
189 | 225 |
} |
190 | 226 |
|
227 |
|
|
228 |
/* |
|
229 |
* prey_FSM - Prey finite state machine which starts by backing away, turning, |
|
230 |
* and then running and avoiding obstacles. |
|
231 |
* |
|
232 |
* Arguments: |
|
233 |
* prey_state - Current prey state. |
|
234 |
* |
|
235 |
* returns - The new state of the prey state machine. |
|
236 |
*/ |
|
237 |
|
|
191 | 238 |
int prey_FSM(int prey_state) { |
192 | 239 |
|
240 |
/* Variable to store the front rangefinder readings */ |
|
193 | 241 |
int rangeVals[3] = {0, 0, 0}; |
194 |
|
|
242 |
|
|
195 | 243 |
switch (prey_state) { |
196 | 244 |
|
197 | 245 |
case PREY_START_BACK: |
... | ... | |
222 | 270 |
rangeVals[1] = range_read_distance(IR2); |
223 | 271 |
rangeVals[2] = range_read_distance(IR3); |
224 | 272 |
|
273 |
/* Drive away if we detect obstacles using the rangefinders */ |
|
225 | 274 |
if (rangeVals[1] > 0 && rangeVals[1] < IR_DIST_THRESHOLD) { |
226 | 275 |
motor_l_set(FORWARD, 255); |
227 | 276 |
motor_r_set(BACKWARD, 255); |
... | ... | |
250 | 299 |
|
251 | 300 |
} |
252 | 301 |
|
302 |
|
|
303 |
/* |
|
304 |
* hunter_FSM - Hunter finite state machine which defaults to spiraling |
|
305 |
* outwards until the BOM can locate the prey. Once the BOM locates |
|
306 |
* the prey, chase the prey as fast as possible. |
|
307 |
* |
|
308 |
* Arguments: |
|
309 |
* hunter_state - Current hunter state. |
|
310 |
* maxBOM - Current maximum BOM value. |
|
311 |
* frontIR - Current front IR rangefinder reading value. |
|
312 |
* |
|
313 |
* returns - The new state of the hunter state machine. |
|
314 |
*/ |
|
315 |
|
|
253 | 316 |
int hunter_FSM(int hunter_state, int maxBOM, int frontIR) { |
254 | 317 |
|
255 | 318 |
switch(hunter_state) { |
... | ... | |
264 | 327 |
} |
265 | 328 |
break; |
266 | 329 |
case HUNTER_CHASE: |
330 |
|
|
331 |
/* A large look-up table to set the speed of the motors based on |
|
332 |
* where the prey robot is. */ |
|
267 | 333 |
if (maxBOM == -1) { |
268 | 334 |
return HUNTER_CHASE; |
269 | 335 |
} else if (maxBOM == 4) { |
Also available in: Unified diff