Colony

#include <dragonfly_lib.h>

//#include "smart_run_around_fsm.h"

// Martin "deffi" Herrmann Test

static void oset(uint8_t r,uint8_t g,uint8_t b)

{

    orbs_set(r,g,b,255-r,255-g,255-b);

}

static void color_circle (void)

{

    while(1)

    {

        for (uint8_t part=0; part<6; ++part)

        {

            for (uint8_t up=0; up<255; ++up)

            {

                uint8_t dn=255-up;

                uint8_t on=255;

                uint8_t of=0;

                if (1)

                {

                    // Maximum brightness mode

                    switch (part)

                    {

                        case 0: oset (on, up, of); break;

                        case 1: oset (dn, on, of); break;

                        case 2: oset (of, on, up); break;

                        case 3: oset (of, dn, on); break;

                        case 4: oset (up, of, on); break;

                        case 5: oset (on, of, dn); break;

                    }

                }

                else

                {

                    // Constant brightness mode (not very constant though)

                    switch (part%3)

                    {

                        case 0: oset (dn, up, of); break;

                        case 1: oset (of, dn, up); break;

                        case 2: oset (up, of, dn); break;

                    }

                }

                delay_ms (2);

            }

        }

    }

}

static void acl(void)

{

#define redval 255

#define greenval 150

#define interval 1300

#define flash 20

#define pause 200

#define def do { orb1_set(redval,0,0); orb2_set(0,greenval,0); } while (0)

#define wht do { orb_set(255,255,255); } while (0)

    while (1)

    {

        wht; delay_ms (flash);

        def; delay_ms (pause-flash);

        wht; delay_ms (flash);

        def; delay_ms (pause-flash);

        def; delay_ms (interval-2*pause-2*flash);

    }

}

int main(void) {

    //  dragonfly_init(ALL_ON);

    dragonfly_init(0);

    usb_init ();

    usb_puts ("Startup\r\n");

    //encoders_init ();

    //analog_init(ADC_START);

    //analog_init(0);

    //range_init();

    //DDRF=2;

    //motors_init ();

    //motor2_set (FORWARD, 64);

    //orb_init_binary ();

    orb_init_pwm ();

    if (false)

    {

        orbs_set (255, 0, 0, 0, 0, 0); delay_ms (500);

        orbs_set (0, 255, 0, 0, 0, 0); delay_ms (500);

        orbs_set (0, 0, 255, 0, 0, 0); delay_ms (500);

        orbs_set (0, 0, 0, 255, 0, 0); delay_ms (500);

        orbs_set (0, 0, 0, 0, 255, 0); delay_ms (500);

        orbs_set (0, 0, 0, 0, 0, 255); delay_ms (500);

    }

    if (false)

    {

        orb_set (1,1,1);

        while (1)

        {

            SYNC

            {

                for (uint8_t m=0; m<100; ++m)

                {

                    _delay_us(10);

                }

            }

        }

    }

    if (false)

    {

        while (1)

        {

            for (uint8_t x=0; x<255; ++x)

            {

                orbs_set (x, 1, 2, 3, 4, 5);

                for (uint8_t n=0; n<80; ++n)

                {

                    SYNC

                    { for (uint8_t m=0; m<1; ++m) _delay_us(50); }

                }

            }

            for (uint8_t x=255; x>0; --x)

            {

                orbs_set (x, 1, 1, 1, 1, 1);

                delay_ms (4);

            }

        }

    }

    if (false)

    {

        //orbs_set (2,4,6,8,10,12);

        //orbs_set (32, 64, 96, 128, 160, 192);

        orbs_set (128,128,128,128,128,128);

        while (1);

    }

    if (false)

    {

        if (!button2_read ())

        {

            orb_set_mode (orb_mode_pwm);

            orb_disable_timer ();

            while (1)

            {

                // Continuously call the sorting routine for timing test

                for (uint8_t i=0; i<200; ++i)

                {

                    orbs_set (10,20,30,40,50,60);

                    delay_ms (10);

                }

                for (uint8_t i=0; i<200; ++i)

                {

                    orbs_set (60,50,40,30,20,10);

                    delay_ms (10);

                }

            }

        }

        else

        {

            while (button2_read ());

            if (button1_read ())

            {

                color_circle ();

            }

            else

            {

                acl ();

            }

        }

    }

    // Do some lighting

    if (!button2_read ())

        color_circle ();

    else

        acl ();

}

# this is a local makefile

# Relative path to the root directory (containing lib directory)

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = main

# Uncomment this to use the wireless library

USE_WIRELESS = 1

# com1 = serial port. Use lpt1 to connect to parallel port.

AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile

CC = gcc

CFLAGS = -Wall -g

all: ref

ref: *.c ../../libwireless/lib/*.c 

	$(CC) $(CFLAGS) *.c -L../../libwireless/lib -o ref -DWL_DEBUG -lwireless -lpthread -ggdb

clean:

	rm -f *.o ref ../lib/*.o

#include <stdlib.h>

#include <stdio.h>

#include <time.h>

#include <unistd.h>

#include "../../libwireless/lib/wl_basic.h"

#include "../../libwireless/lib/wireless.h"

#include "../hunter_prey.h"

#define TYPE 42	// packet type for wireless communication

#define NUM_BOTS 17

#define TOTAL_GAME_TIME 600*5 //in 100 ms intervals

void packet_receive(char type, int source, unsigned char* packet, int length);

volatile unsigned int preyTime[20];

volatile int preyBot=-1;

void waitKey(void) {

  unsigned char buf[10];

  //read from stdin

   while(read(0, buf, 10)==10) {

     printf("!");

     fflush(stdout);

   }

  while(getchar()==-1);

}

void print_stats(void) {

  int i=0;

  for(i=0;i<NUM_BOTS;i++) {

    printf("%5d ", preyTime[i]);

  }

  printf("\n");

}

int main(int argc, char *argv[])

{

    char send_buffer[2];    // holds data to send

    int data_length;	// length of data received

    unsigned char *packet_data;	// data received

    int ret;

    int i;

    int winner, maxTime=0;

    unsigned int channel = 0xF;

    struct timespec delay, rem;

    delay.tv_sec = 0;

    delay.tv_nsec = 100000000;

    if(argc > 1) {

      channel = atoi(argv[1]);

    }

    printf("using wireless channel %d\n", channel);

    wl_set_com_port("/dev/ttyUSB0");

    // set up wireless

    ret = wl_basic_init(&packet_receive);

    if(ret) {

      printf("ERROR: wl_basic_init failed. %d\n", ret);

      return ret;

    }

    //wl_set_channel(channel);

    for(i=0;i<NUM_BOTS;i++) {

      preyTime[i]=0;

    }

    ret = 0;    

    i=0;

    printf("ready\n");

    //TODO: use rtc to make this more accurate

    while(i < TOTAL_GAME_TIME) {

      wl_do();

      if(ret==0) {

	ret = nanosleep(&delay, &rem);

      }

      else {

	ret = nanosleep(&rem, &rem);

      }

      //update counts when we have waited at least enough time

      if(ret == 0 && preyBot >= 0) {

	preyTime[preyBot]++;

	if(i==0) { //time starts on the first real tag

	  printf("\nThe race is on!\n");

	}

	i++;

      }

      if((TOTAL_GAME_TIME - i)%600 == 0) {

	printf("%d minutes remaining!\n", (TOTAL_GAME_TIME - i)/600);

      }

      if(i%10==0) {

	print_stats();

	if( TOTAL_GAME_TIME - i <= 100)

	  printf("%d...\n", (TOTAL_GAME_TIME - i)/10);

      }

    }

    for(i=1;i<NUM_BOTS;i++) {

      if(preyTime[i] > maxTime) {

	maxTime = preyTime[i];

	winner = i;

      }

    }

    printf("\n\GAME OVER!n\nAAAAAAAAAAND the winner is\nBOT %d!!!!\n", winner);

    return 0;

}

void packet_receive(char type, int source, unsigned char* packet, int length)

{

  if(type==42 && length>=2){

    if(packet[0] == HUNTER_PREY_ACTION_TAG) {

      //TODO: check tag

    }

    else if(packet[0] == HUNTER_PREY_ACTION_ACK) {

      preyBot = packet[1];

      printf("bot %d is prey!\n", preyBot);

    }

    else {

      printf("got invalid packet! %c%d\n", packet[0], packet[1]);

    }

  }

  else {

    printf("got unknown packet! type=%d, length=%d\n", type, length);

  }

}

# this is a local makefile

# Relative path to the root directory (containing lib directory)

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = main

# Uncomment this to use the wireless library

USE_WIRELESS = 1

# com1 = serial port. Use lpt1 to connect to parallel port.

AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile

# this is a local makefile

# Relative path to the root directory (containing lib directory)

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = main

# Uncomment this to use the wireless library

USE_WIRELESS = 1

# com1 = serial port. Use lpt1 to connect to parallel port.

AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile

/*

 * 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"

#include "encoders.h"

#define WL_CHANNEL 					15

#define BACK_THRESHOLD 			-5000

#define TURN_DIST						1024

#define IR_DIST_THRESHOLD		200

/* State Macros */

/* Top Level FSM States */

#define TOP_INIT						0

#define TOP_HUNTER_HUNT			1

#define TOP_HUNTER_TAG			2

#define TOP_HUNTER_PURSUE		3

#define TOP_PREY_AVOID			4

#define TOP_HUNTER_WAIT			5

#define TOP_ERROR						6

/* Hunter FSM States */

#define HUNTER_SPIRAL				0

#define HUNTER_CHASE				1

/* Prey FSM States */

#define PREY_START_BACK			0

#define PREY_BACKING				1

#define PREY_TURN						2

#define PREY_AVOID					3

/* Function prototype declarations */

int hunter_FSM(int, int, int);

int prey_FSM(int);

/* Variables used to receive packets */

unsigned char* packet_data;

int data_length;

/*  Data buffer used to send packets */

char send_buffer[1];

int main(void)

{

    /* Initialize dragonfly board */

    dragonfly_init(ALL_ON);

		xbee_init();

		encoders_init();

    /* Initialize the basic wireless library */

    wl_basic_init_default();

    /* Set the XBee channel to assigned channel */

    wl_set_channel(WL_CHANNEL);

    /* ****** CODE HERE ******* */

		/* Initialize state machines */

		int state = TOP_INIT;

		int hunter_state = HUNTER_SPIRAL;

		int prey_state = PREY_AVOID;

		int frontIR = 0;

		int maxBOM = 0;

		int robotID = get_robotid();

		int oldTime = 0, curTime = 0;

		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(RED, GREEN);

					delay_ms(500);

					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;

					} else {

						state = TOP_HUNTER_HUNT;

						hunter_state = HUNTER_SPIRAL;

					}

					break;

				case TOP_HUNTER_HUNT:

					orbs_set_color(RED, RED);

					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);

						}

					}

					break;

				case TOP_HUNTER_TAG:

					orbs_set_color(RED, PURPLE);

					delay_ms(500);

					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;

					}

					break;

				case TOP_HUNTER_PURSUE:

					orbs_set_color(RED, BLUE);

					curTime = rtc_get();

					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 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();

						hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);

					}

					break;

				case TOP_PREY_AVOID:

					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];

						wl_basic_send_global_packet(42, send_buffer, 2);

						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);

					bom_off();

					motors_off();

					delay_ms(5000);

					state = TOP_HUNTER_HUNT;

					hunter_state = HUNTER_SPIRAL;

					break;

				case TOP_ERROR:

				default:

					orbs_set_color(PURPLE, PURPLE);

					state = TOP_ERROR;

					while(1);

					break;

			}

		}

    /* ****** END HERE ******* */

    while(1);

    return 0;

}

/*

 * 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:

			motor_l_set(BACKWARD, 255);

			motor_r_set(BACKWARD, 255);

			encoder_rst_dx(LEFT);

			return PREY_BACKING;

			break;

		case PREY_BACKING:

			if (encoder_get_x(LEFT) < BACK_THRESHOLD) {

				motor_l_set(BACKWARD, 255);

				motor_r_set(FORWARD, 255);

				encoder_rst_dx(RIGHT);

				return PREY_TURN;

			} else {

				return PREY_BACKING;

			}

			break;

		case PREY_TURN:

			if (encoder_get_x(RIGHT) > TURN_DIST) {

				return PREY_AVOID;				

			} else {

				return PREY_TURN;

			}

			break;

		case PREY_AVOID:

			rangeVals[0] = range_read_distance(IR1);

			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);

				return PREY_AVOID;

			} else if (rangeVals[0] > 0 && rangeVals[0] < IR_DIST_THRESHOLD) {

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 170);

				return PREY_AVOID;

			} else if (rangeVals[2] > 0 && rangeVals[2] < IR_DIST_THRESHOLD) {

				motor_l_set(FORWARD, 170);

				motor_r_set(FORWARD, 255);

				return PREY_AVOID;

			} else {			

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 255);

				return PREY_AVOID;

			}

			break;

		default:

			return PREY_AVOID;

			break;

	}

	return prey_state;

}

/*

 * 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) {

		case HUNTER_SPIRAL:

			if (maxBOM != -1) {

				return HUNTER_CHASE;

			} else {

				motor_l_set(FORWARD, 170);

				motor_r_set(FORWARD, 190);

				return HUNTER_SPIRAL;

			}

			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) {

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 255);

				return HUNTER_CHASE;

			} else if (maxBOM == 3) {

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 240);

				return HUNTER_CHASE;

			} else if (maxBOM == 5) {

				motor_l_set(FORWARD, 240);

				motor_r_set(FORWARD, 255);

				return HUNTER_CHASE;

			} else if (maxBOM < 3) {

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 170);

				return HUNTER_CHASE;

			} else if (maxBOM > 5 && maxBOM <= 8) {

				motor_l_set(FORWARD, 170);

				motor_r_set(FORWARD, 255);

				return HUNTER_CHASE;

			} else if (maxBOM > 8 && maxBOM < 12) {

				motor_l_set(BACKWARD, 255);

				motor_r_set(FORWARD, 255);

				return HUNTER_CHASE;

			} else {

				motor_l_set(FORWARD, 255);

				motor_r_set(BACKWARD, 255);

				return HUNTER_CHASE;

			}

			break;

		default:

			return HUNTER_SPIRAL;

			break;

	}

	return hunter_state;

}

#include "hunter_prey.h"

#include <dragonfly_lib.h>

#define TAG_TIME 3

#define TAG_RANGE 200

/**** This file should not be edited! ****/

/*

 * The criteria for tagging are the following:

 *   * The max bom is betweed 1 and 7

 *   * The front rangefinder reads less than TAG_RANGE

 *   * -1 values from the rangefinder are ignored

 *   * These conditions are met across TAG_TIME calls to this function

 */

unsigned char hunter_prey_tagged(int max_bom, int frontRange) {

  static int onTarget = 0;

  if(max_bom < 7 && max_bom > 1 && frontRange > 0 && frontRange < TAG_RANGE) {

    if(onTarget == 0) {

      onTarget = TAG_TIME;

      usb_puts("On target!\n");

    }

    else {

      if(--onTarget <= 0) {

	onTarget = 0;

	usb_puts("TAG!\n");

	return 1;

      }

    }

  }

  else{

    //don't reset onTarget because the robot got too close

    if(frontRange > 0)

      onTarget = 0;

  }

  return 0;

}

#ifndef _HUNTER_PREY_H

#define _HUNTER_PREY_H

#include <inttypes.h>

/**** This file should not be edited! ****/

/*

 * The packet structure is 2 bytes

 * byte 0 is the action, which is one of the values below

 * byte 1 is the robot id

 */

#define HUNTER_PREY_ACTION_TAG 'T'

#define HUNTER_PREY_ACTION_ACK 'A'

unsigned char hunter_prey_tagged(int max_bom, int front_rangefinder);

#endif

/* testbench for Lab 2 first checkpoint

 * determine whether robot under test complies with communication standard

 * to be conducted using XBee USB dongle

 */

/* The tests shall be as follows

 * 1. Receive a TAG, send an ACK

 * - Robot passes if it sends a tag and changes to prey state

 * 2. Send a TAG, receive an ACK

 * - Pass if sends ACK, changes to wait state, then hunter state

 * 3. Receive TAG, send slightly delayed (< 1s) ACK

 * - Pass if sends one and only one TAG packet (and changes state appropriately)

 * 4. Receive TAG, do not send ACK

 * - Pass if sends one and only one TAG packet (and does not change state)

 * 5. Receive TAG, send ACK to incorrect robot

 * - Pass if goes to wait state, then back to hunter state

 * 6. Simulate TAG from robot A to B, ACK from B to A

 * - Pass if ignores TAG, goes to wait then hunter in response to ACK

 */

#include <stdlib.h>

#include <stdio.h>

#include <unistd.h>

#include "../../libwireless/lib/wl_basic.h"

#include "../../libwireless/lib/wireless.h"

#include "hunter_prey.h"

#include <time.h>

#define TYPE 42	// packet type for wireless communication

#define ROBOTID 255 // make up a robot id because the PC doesn't have one

void waitKey() {

  unsigned char buf[10];

  //read from stdin

   while(read(0, buf, 10)==10) {

     printf("!");

     fflush(stdout);

   }

  while(getchar()==-1);

}

int main(int argc, char *argv[])

{

    char send_buffer[2];    // holds data to send

    int data_length;	// length of data received

    unsigned char *packet_data;	// data received

    int ret;

    unsigned int channel = 0xF;

    struct timespec delay8, rem;

    if(argc > 1) {

      channel = atoi(argv[1]);

    }

    printf("using wireless channel %d\n", channel);

    delay8.tv_sec = 2;

    delay8.tv_nsec = 800000000;

    wl_set_com_port("/dev/ttyUSB0");

    // set up wireless

    ret = wl_basic_init_default();

    if(ret) {

      printf("ERROR: wl_basic_init_default returned %d\n", ret);

      return ret;

    }

    ret = wl_set_channel(channel);

    if(ret) {

      printf("ERROR: set channel failed! %d\n", ret);

      return ret;

    }

    printf("Testing communications\n\n");

    // Receive TAG, send ACK

    printf("Receive TAG, send ACK... ");

    fflush(stdout);

    // Wait until we receive a packet

    while (!(packet_data = wl_basic_do_default(&data_length)));

    printf("got packet, validating and sending ack...");

    fflush(stdout);

    if (data_length > 2)

    {

	printf("Excessive TAG packet length... ");

	fflush(stdout);

    }

    if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)

    {

	// send back an ACK

	send_buffer[0] = HUNTER_PREY_ACTION_ACK;

	send_buffer[1] = packet_data[1];

	printf("sending ack intended for robot %d\n", packet_data[1]);

	fflush(stdout);

	wl_basic_send_global_packet(TYPE, send_buffer, 2);

	printf("PASSED\n");

    }

    else

	printf("FAILED\n");

    printf("robot should be prey\nPress enter to continue...\n");

    waitKey();

    // Send a TAG, receive an ACK

    printf("Send TAG, wait for ACK... ");

    fflush(stdout);

    send_buffer[0] = HUNTER_PREY_ACTION_TAG;

    send_buffer[1] = ROBOTID;

    // robot number stays the same

    wl_basic_send_global_packet(TYPE, send_buffer, 2);

    // Wait for an ACK

    ret = nanosleep(&delay8, &rem);  // wait for 800 ms before sending ACK

    while(ret) {

      //      printf("nanosleep not done, needs to wait %ld more\n", rem.tv_nsec);

      ret = nanosleep(&rem, &rem);

    }

    data_length = -1;

    packet_data = wl_basic_do_default(&data_length);

    // Check ACK for presence and correctness

    if (!packet_data || data_length < 2 ||

	    packet_data[0] != HUNTER_PREY_ACTION_ACK ||

        packet_data[1] != ROBOTID) {

      printf("FAILED, packet=%p, len=%d\n", packet_data, data_length);

      if(data_length >= 2) {

	printf("\tpacket = [%c, %d]\n",packet_data[0], packet_data[1]);

      }

    }

    else

	printf("PASSED\n");

    printf("Robot should be Hunter\nPress enter to continue...\n");

    waitKey();

    // Receive a TAG, send a delayed ACK

    printf("Receive a TAG, send a delayed ACK... ");

    fflush(stdout);

    while (!(packet_data = wl_basic_do_default(&data_length)));

    if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)

    {

        printf("got TAG, waiting...");

	fflush(stdout);

	// wait before sending ACK back

	//usleep(900000);

        delay8.tv_sec = 0;

        ret = nanosleep(&delay8, &rem);  // wait for 800 ms before sending ACK

	while(ret) {

	  printf(".");

	  fflush(stdout);//      printf("nanosleep not done, needs to wait %ld more\n", rem.tv_nsec);

	  ret = nanosleep(&rem, &rem);

	}

	if (wl_basic_do_default(&data_length))

	    printf("FAILED, got another TAG too soon\n");

	else

	{

	    // send packet

	    send_buffer[0] = HUNTER_PREY_ACTION_ACK;

	    send_buffer[1] = packet_data[1];

	    wl_basic_send_global_packet(TYPE, send_buffer, 2);

	    printf("PASSED\n");

	}

    }

    else

	printf("FAILED\n");

    printf("robot should be prey.\n Press Enter...\n");

    waitKey();

    printf("sending courtesy tag...");

    fflush(stdout);

    send_buffer[0] = HUNTER_PREY_ACTION_TAG;

    send_buffer[1] = ROBOTID;

    // robot number stays the same

    wl_basic_send_global_packet(TYPE, send_buffer, 2);

    printf("done.\nwaiting for ack...");

    fflush(stdout);

    while (!(packet_data = wl_basic_do_default(&data_length)));

    if (!packet_data || data_length < 2 ||

	    packet_data[0] != HUNTER_PREY_ACTION_ACK ||

	    packet_data[1] != ROBOTID)

      printf("FAILED, packet=%p, len=%d\n", packet_data, data_length);

    else

	printf("done\n");

    // Receive a TAG, never send an ACK

    printf("Receive TAG, never send ACK... ");

    fflush(stdout);

    while (!(packet_data = wl_basic_do_default(&data_length)));

    if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)

    {

        printf("got TAG, monitoring for 5 seconds\n");

	fflush(stdout);

	delay8.tv_sec = 5;

	delay8.tv_nsec = 0;

	ret = nanosleep(&delay8, &rem);  // wait for 5 secs

	while(ret) {

	  //      printf("nanosleep not done, needs to wait %ld more\n", rem.tv_nsec);

	  ret = nanosleep(&rem, &rem);

	}

	if (wl_basic_do_default(&data_length))

	    printf("FAILED, robot is spamming\n");

	else

	    printf("PASSED\n");

    }

    else

	printf("FAILED\n");

    printf("robot should be hunter.\nPress enter...\n");

    waitKey();

    // Receive TAG, send ACK to incorrect robot

    printf("Receive TAG, send ACK to incorrect robot... ");

    fflush(stdout);

    // Wait until we receive a packet

    while (!(packet_data = wl_basic_do_default(&data_length)));

    if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)

    {

	// send back an ACK

	send_buffer[0] = HUNTER_PREY_ACTION_ACK;

	send_buffer[1] = packet_data[1] + 1;

	wl_basic_send_global_packet(TYPE, send_buffer, 2);

	printf("PASSED\n");

    }

    else

	printf("FAILED\n");

    printf("robot should wait and then still be hunter.\nPress Enter...\n");

    waitKey();

    // Simulate TAG from robot A to B, ACK from robot B to A

    printf("Send TAG from robot A to B, ACK from B to A... ");

    fflush(stdout);

    // TAG

    send_buffer[0] = HUNTER_PREY_ACTION_TAG;

    send_buffer[1] = packet_data[1] - 1;    // robot other than testee

    wl_basic_send_global_packet(TYPE, send_buffer, 2);

    // ACK

    send_buffer[0] = HUNTER_PREY_ACTION_ACK;

    // send_buffer[1] stays the same

    wl_basic_send_global_packet(TYPE, send_buffer, 2);

    if (wl_basic_do_default(&data_length))

	printf("FAILED\n");

    else

	printf("PASSED\n");

    printf("robot should be waiting\n");

    return 0;

}

#ifndef _HUNTER_PREY_H

#define _HUNTER_PREY_H

#include <inttypes.h>

/*

 * The packet structure is 2 bytes

 * byte 0 is the action, which is one of the values below

 * byte 1 is the robot id

 */

#define HUNTER_PREY_ACTION_TAG 'T'

#define HUNTER_PREY_ACTION_ACK 'A'

uint8_t hunter_prey_tagged(int max_bom, int front_rangefinder);

#endif

CC = gcc

CFLAGS = -Wall -g

all: testbench

testbench: *.c ../../libwireless/lib/*.c 

	$(CC) $(CFLAGS) *.c -L../../libwireless/lib -o testbench -DWL_DEBUG -lwireless -lpthread -ggdb

clean:

	rm -f *.o testbench ../lib/*.o

#include <dragonfly_lib.h>

#include "smart_run_around_fsm.h"

void run_around_init(void)

{

  //range_init();

  //orb_init();

  state = SRA_FORWARD;  // start out moving forward

  // initialize rangefinder values to 0

  d1 = 0, d2 = 0, d3 = 0, d4 = 0, d5 = 0;

}

/* evaluate rangefinder input and update state

 * call this function as often as possible to avoid collisions

 */

void run_around_FSM(void)

{

    /* TODO: find a better way to handle rangefinder input

     * robot should deal with -1s (obstacles too close or too far to detect)

     * in a way that keeps it from crashing or driving in circles

     */

    // do not update distances when rangefinders return -1

    // otherwise update distances with new rangefinder values

    int16_t temp;

    temp = range_read_distance(IR1);

    d1 = (temp == -1) ? d1 : temp;

    temp = range_read_distance(IR2);

    d2 = (temp == -1) ? d2 : temp;

    temp = range_read_distance(IR3);

    d3 = (temp == -1) ? d3 : temp;

    temp = range_read_distance(IR4);

    d4 = (temp == -1) ? d4 : temp;

    temp = range_read_distance(IR5);

    d5 = (temp == -1) ? d5 : temp;

    // update state based on rangefinder input

    if (d2 < IR2_DANGER)    // avoid frontal collision by turning in place

	state = SRA_SPIN;

    /* nowhere to turn, so don't

     * will probably need to turn around soon, but not yet

     */

    else if (d1 < IR13_DANGER && d3 < IR13_DANGER)

	state = SRA_FORWARD;

    // avoid left-side collision by turning right

    else if (d1 < IR13_DANGER || d5 < IR45_DANGER)

	state = SRA_RIGHT;

    // avoid right-side collision by turning left

    else if (d3 < IR13_DANGER || d4 < IR45_DANGER)

	state = SRA_LEFT;

    else if (d2 < IR2_INTEREST)	// should turn to avoid obstacle up ahead

    {

	if (d3 >= d1)	// more room on right

	    state = SRA_RIGHT;

	else	// more room on left

	    state = SRA_LEFT;

    }

    else    // no obstacles close by, so keep going straight

	state = SRA_FORWARD;

    /* Debugging via USB output */

/*     usb_puts("IR1: "); */

/*     usb_puti(d1); */

/*     usb_puts(" IR2: "); */

/*     usb_puti(d2); */

/*     usb_puts(" IR3: "); */

/*     usb_puti(d3); */

/*     usb_puts(" IR4: "); */

/*     usb_puti(d4); */

/*     usb_puts(" IR5: "); */

/*     usb_puti(d5); */

/*     usb_puts("\n\r"); */

    evaluate_state();	// take action on updated state

}

// behave according to current state

// TODO: adjust speeds after testing

void evaluate_state(void)

{

    switch (state)

    {

	case SRA_FORWARD:

	    //orb_set_color(GREEN);

	    move(220, 0);	// drive straight forward

	    break;

	case SRA_LEFT:

	    //orbs_set_color(GREEN, YELLOW);  // green on left side

	    move(HALF_SPD, NRML_TURN);	// drive forward and to the left

	    break;

	case SRA_RIGHT:

	    //orbs_set_color(YELLOW, GREEN);  // green on right side

	    move(HALF_SPD, -NRML_TURN);	// drive forward and to the right

	    break;

	case SRA_SPIN:

	    //orb_set_color(RED);

	    move(0, NRML_TURN);	// spin CCW without traveling

	    break;

	default:    // should never reach this

	    //orb_set_color(PURPLE);

	    move(0, 0);	// stop completely

    }

}

#ifndef _HUNTER_PREY_H

#define _HUNTER_PREY_H

#include <inttypes.h>

/*

 * The packet structure is 2 bytes

 * byte 0 is the action, which is one of the values below

 * byte 1 is the robot id

 */

#define HUNTER_PREY_ACTION_TAG 'T'

#define HUNTER_PREY_ACTION_ACK 'A'

uint8_t hunter_prey_tagged(int max_bom, int front_rangefinder);

#endif

// smart_run_around_fsm.h

// required to run Smart Run Around functions

// declare functions and global variables

#ifndef _RUN_AROUND_FSM_H_

#define _RUN_AROUND_FSM_H_

// states (robot shall never move in reverse)

// these constants may be changed (but can't overlap) without effect

#define SRA_FORWARD 0   // drive straight forward

#define SRA_LEFT 1	// drive forward and to the left

#define SRA_RIGHT 2	// drive forward and to the right

#define SRA_SPIN 3	// turn without traveling (last resort if otherwise stuck)

/* conditions on rangefinders (in mm) informing state changes

 * distances are 50 greater than in real life, cannot be smaller than 100

 * naming format: SENSOR_URGENCY

 * changing these constants affects robot operation

 */

#define IR2_DANGER 150	// 10 cm: dangerously close to front

#define IR13_DANGER 200 // 15 cm: dangerously close to sides (looking ahead)

#define IR2_INTEREST 300    // 25 cm: notably close to front

#define IR45_DANGER 150	// dangerously close to side (looking sideways)

uint8_t state;	// current state of FSM: SRA_FORWARD, SRA_LEFT, SRA_RIGHT, SRA_SPIN

int16_t d1, d2, d3, d4, d5; // values returned by rangefinders

// initialize rangefinders and global variables

void run_around_init(void);

/* evaluate rangefinder input and update state

 * call this function as often as possible to avoid collisions

 */

void run_around_FSM(void);

// behave according to current state

void evaluate_state(void);

#endif

# this is a local makefile

# Relative path to the root directory (containing lib directory)

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = main

# Uncomment this to use the wireless library

USE_WIRELESS = 1

# com1 = serial port. Use lpt1 to connect to parallel port.

AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_basic.h>

#include "smart_run_around_fsm.h"

#include "hunter_prey.h"

#define CHAN 0xC

#define ACK_PAUSE 5000

#define ROBOT_HUNTER 0

#define ROBOT_PREY 1

#define ROBOT_TAGGED 2

#define ROBOT_IDLE 3

void packet_receive(unsigned char* packet, int length);

volatile uint8_t robotState; // current state

volatile uint8_t tagger;    // id of tagging robot

uint8_t id; // my robot's id

void test_bom(void) {

  int i;

  int val;

  while(1) {

    bom_refresh(BOM_ALL);

    for(i=0; i<16; i++) {

      val = bom_get(i);

      usb_puti(val);

      usb_putc(' ');

    }

    usb_puts("| ");

    usb_puti(range_read_distance (IR1));

    usb_putc(' ');

    usb_puti(range_read_distance (IR2));

    usb_putc(' ');

    usb_puti(range_read_distance (IR3));

    usb_putc(' ');

    usb_puti(range_read_distance (IR4));