/ - Diff - Colony - Robotics Club

Revision 1500

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

     /*
      * Hunter-Prey main.c File - Implementation of Hunter-Prey behavior which
      * 		uses finite state machines to manage the behavior. A top level
      * 		state machine controls the high level behavior switches between
      * 		"hunter" and "prey" and manages the wireless communication. Two
      * 		additional state machines control the behavior of the robot when
      * 		it is in "prey" mode and when it is in "hunter" mode.
+     *
      * Author: John Sexton, Colony Project, CMU Robotics Club
      */
     #include <dragonfly_lib.h>
     #include <wl_basic.h>
     #include "hunter_prey.h"
-...
     /* State Macros */
     /* Top Level FSM */
     /* Top Level FSM States */
     #define TOP_INIT						0
     #define TOP_HUNTER_HUNT			1
     #define TOP_HUNTER_TAG			2
-...
     #define TOP_HUNTER_WAIT			5
     #define TOP_ERROR						6
     /* Hunter FSM */
     /* Hunter FSM States */
     #define HUNTER_SPIRAL				0
     #define HUNTER_CHASE				1
     /* Prey FSM */
     /* Prey FSM States */
     #define PREY_START_BACK			0
     #define PREY_BACKING				1
     #define PREY_TURN						2
-...
     		while (1) {
     			/* Check if we've received a wireless packet */
     			packet_data = wl_basic_do_default(&data_length);
     			/* Top level state machines */
     			switch(state) {
     				case TOP_INIT:
-...
     					orbs_set_color(GREEN, RED);
     					delay_ms(500);
     					/* Allow user to pick the starting behavior */
     					if (button1_read()) {
     						state = TOP_PREY_AVOID;
     						prey_state = PREY_AVOID;
-...
     					if (packet_data && data_length == 2
     							&& packet_data[0] == HUNTER_PREY_ACTION_ACK) {
     						/* If we've received an ACK, we need to wait */
     						state = TOP_HUNTER_WAIT;
     					} else {
     						/* Record some sensor readings and check if we can TAG */
     						bom_refresh(BOM_ALL);
     						frontIR = range_read_distance(IR2);
     						maxBOM = get_max_bom();
     						if (hunter_prey_tagged(maxBOM, frontIR) || button1_read()) {
     							state = TOP_HUNTER_TAG;
     						} else {
     							/* If we haven't tagged, then enter hunter FSM */
     							hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);
+    						}
+    					}
-...
     					if (packet_data && data_length == 2
     							&& packet_data[0] == HUNTER_PREY_ACTION_ACK) {
     						/* If we've received an ACK, then someone beat us to the TAG and
      						 * we need to wait. */
     						state = TOP_HUNTER_WAIT;
     					} else {
     						/* Prepare and send the TAG packet */
     						send_buffer[0] = HUNTER_PREY_ACTION_TAG;
     						send_buffer[1] = robotID;
     						wl_basic_send_global_packet(42, send_buffer, 2);
     						/* Record the time so we don't spam a TAG message on the network */
     						oldTime = rtc_get();
     						state = TOP_HUNTER_PURSUE;
+    					}
-...
     					if (packet_data && data_length == 2
     							&& packet_data[0] == HUNTER_PREY_ACTION_ACK) {
     						/* Check if we've received a new wireless packet */
     						if (packet_data[1] == robotID) {
     							/* We've been ACKed, so we can now become the prey */
     							state = TOP_PREY_AVOID;
     							prey_state = PREY_START_BACK;
     						} else {
     							/* If we get an ACK with a different robotID, then someone beat us
     							 * to the TAG, so we must wait */
     							state = TOP_HUNTER_WAIT;
+    						}
     					} else if (curTime - oldTime > 1) {
     						/* If 1 second has ellapsed, return to normal hunting state (we can
     						 * TAG again now) */
     						state = TOP_HUNTER_HUNT;
     					} else if (oldTime > curTime) {		// If overflow or was reset, restart
     					} else if (oldTime > curTime) {
     						/* If for some reason the timer overflows, or the wireless library
     						 * (which is also using the same timer) resets the timer,
     						 * reinitialize the timer so that we don't wait too long for the
     						 * timer to catch back up. */
     						oldTime = curTime;
     					} else {
     						/* If no other behavioral changes need to be made, then continue
     						 * with the hunter FSM where we left off */
     						bom_refresh(BOM_ALL);
     						frontIR = range_read_distance(IR2);
     						maxBOM = get_max_bom();
-...
     					orbs_set_color(GREEN, GREEN);
     					if (packet_data && data_length == 2
     							&& packet_data[0] == HUNTER_PREY_ACTION_TAG) {
     						/* Check if we've received a TAG yet. If so then send an ACK back */
     						send_buffer[0] = HUNTER_PREY_ACTION_ACK;
     						send_buffer[1] = packet_data[1];
-...
     						motors_off();
     						state = TOP_HUNTER_WAIT;
     					} else {
     						/* If we haven't received a TAG yet, continue with prey FSM */
     						bom_on();
     						prey_state = prey_FSM(prey_state);
+    					}
     					break;
     				case TOP_HUNTER_WAIT:
     					/* Set orb colors and wait to give the prey the 5 second head start */
     					orbs_set_color(BLUE, BLUE);
     					delay_ms(5000);
     					state = TOP_HUNTER_HUNT;
-...
+    }
     /*
      * prey_FSM - Prey finite state machine which starts by backing away, turning,
      * 		and then running and avoiding obstacles.
+     *
      * Arguments:
      * 	prey_state - Current prey state.
+     *
      * returns - The new state of the prey state machine.
      */
     int prey_FSM(int prey_state) {
     	/* Variable to store the front rangefinder readings */
     	int rangeVals[3] = {0, 0, 0};
     	switch (prey_state) {
     		case PREY_START_BACK:
-...
     			rangeVals[1] = range_read_distance(IR2);
     			rangeVals[2] = range_read_distance(IR3);
     			/* Drive away if we detect obstacles using the rangefinders */
     			if (rangeVals[1] > 0 && rangeVals[1] < IR_DIST_THRESHOLD) {
     				motor_l_set(FORWARD, 255);
     				motor_r_set(BACKWARD, 255);
-...
+    }
     /*
      * hunter_FSM - Hunter finite state machine which defaults to spiraling
      * 		outwards until the BOM can locate the prey. Once the BOM locates
      * 		the prey, chase the prey as fast as possible.
+     *
      * Arguments:
      * 	hunter_state - Current hunter state.
      * 	maxBOM - Current maximum BOM value.
      * 	frontIR - Current front IR rangefinder reading value.
+     *
      * returns - The new state of the hunter state machine.
      */
     int hunter_FSM(int hunter_state, int maxBOM, int frontIR) {
     	switch(hunter_state) {
-...
+    			}
     			break;
     		case HUNTER_CHASE:
     			/* A large look-up table to set the speed of the motors based on
     			 * where the prey robot is. */
     			if (maxBOM == -1) {
     				return HUNTER_CHASE;
     			} else if (maxBOM == 4) {

     /*
      * template.c - A starting point for developing behaviors using the Colony
      * 		robots. To create a new behavior, you should copy this "template"
      * 		folder to another folder and rename the "template.c" file
      * 		appropriately.
+     *
      * This template will have the robot drive in circles and flash the orbs.
+     *
      * Author: John Sexton, Colony Project, CMU Robotics Club
      */
     #include <dragonfly_lib.h>
     #include <wl_basic.h>
     /* Time delay which determines how long the robot circles before it
      * changes direction. */
     #define TIME_DELAY 5000
     int main (void) {
     	/* Initialize the dragonfly boards, the xbee, and the encoders */
     	dragonfly_init(ALL_ON);
     	xbee_init();
     	encoders_init();
     	while (1) {
     		/* Drive left, set orbs, and wait */
     		orbs_set_color(RED, GREEN);
     		motor_l_set(FORWARD, 160);
     		motor_r_set(FORWARD, 255);
     		delay_ms(TIME_DELAY);
     		/* Drive right, change orb colors, and wait */
     		orbs_set_color(PURPLE, BLUE);
     		motor_l_set(FORWARD, 255);
     		motor_r_set(FORWARD, 160);

Also available in: Unified diff

Project

General

Profile

Colony

Revision 1500