Project

General

Profile

Revision 1500

Commented the template file and my implementation of Hunter-Prey to make the code easier to follow.

View differences:

trunk/code/behaviors/hunter_prey/john/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) {
trunk/code/behaviors/template/main.c
1
/*
2
 * template.c - A starting point for developing behaviors using the Colony
3
 * 		robots. To create a new behavior, you should copy this "template"
4
 * 		folder to another folder and rename the "template.c" file
5
 * 		appropriately.
6
 *
7
 * This template will have the robot drive in circles and flash the orbs.
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

  
15
/* Time delay which determines how long the robot circles before it
16
 * changes direction. */
4 17
#define TIME_DELAY 5000
5 18

  
6 19
int main (void) {
7 20

  
21
	/* Initialize the dragonfly boards, the xbee, and the encoders */
8 22
	dragonfly_init(ALL_ON);
9 23
	xbee_init();
10 24
	encoders_init();
11 25
	
12 26
	while (1) {
27
		/* Drive left, set orbs, and wait */
13 28
		orbs_set_color(RED, GREEN);
14 29
		motor_l_set(FORWARD, 160);
15 30
		motor_r_set(FORWARD, 255);
16 31
		delay_ms(TIME_DELAY);
17 32

  
33
		/* Drive right, change orb colors, and wait */
18 34
		orbs_set_color(PURPLE, BLUE);
19 35
		motor_l_set(FORWARD, 255);
20 36
		motor_r_set(FORWARD, 160);

Also available in: Unified diff