Colony

// 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

########Update This Section########

#

#

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

ifndef COLONYROOT

COLONYROOT = ../../..

endif

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

#

###################################

# Hey Emacs, this is a -*- makefile -*-

#----------------------------------------------------------------------------

# WinAVR Makefile Template written by Eric B. Weddington, J?rg Wunsch, et al.

#

# Released to the Public Domain

#

# Additional material for this makefile was written by:

# Peter Fleury

# Tim Henigan

# Colin O'Flynn

# Reiner Patommel

# Markus Pfaff

# Sander Pool

# Frederik Rouleau

#

#----------------------------------------------------------------------------

# On command line:

#

# make all = Make software.

#

# make clean = Clean out built project files.

#

# make coff = Convert ELF to AVR COFF.

#

# make extcoff = Convert ELF to AVR Extended COFF.

#

# make program = Download the hex file to the device, using avrdude.

#                Please customize the avrdude settings below first!

#

# make debug = Start either simulavr or avarice as specified for debugging,

#              with avr-gdb or avr-insight as the front end for debugging.

#

# make filename.s = Just compile filename.c into the assembler code only.

#

# make filename.i = Create a preprocessed source file for use in submitting

#                   bug reports to the GCC project.

#

# To rebuild project do "make clean" then "make all".

#----------------------------------------------------------------------------

#if you want your code to work on the Firefly++ and not Firefly+

#then add the -DFFPP line to CDEFS

CDEFS =

#-DFFPP

# MCU name

MCU = atmega128

# Processor frequency.

#     This will define a symbol, F_CPU, in all source code files equal to the

#     processor frequency. You can then use this symbol in your source code to

#     calculate timings. Do NOT tack on a 'UL' at the end, this will be done

#     automatically to create a 32-bit value in your source code.

F_CPU = 8000000

# Output format. (can be srec, ihex, binary)

FORMAT = ihex

# List C source files here. (C dependencies are automatically generated.)

SRC = $(wildcard *.c)

# List Assembler source files here.

#     Make them always end in a capital .S.  Files ending in a lowercase .s

#     will not be considered source files but generated files (assembler

#     output from the compiler), and will be deleted upon "make clean"!

#     Even though the DOS/Win* filesystem matches both .s and .S the same,

#     it will preserve the spelling of the filenames, and gcc itself does

#     care about how the name is spelled on its command-line.

ASRC =

# Optimization level, can be [0, 1, 2, 3, s].

#     0 = turn off optimization. s = optimize for size.

#     (Note: 3 is not always the best optimization level. See avr-libc FAQ.)

OPT = s

# Debugging format.

#     Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs.

#     AVR Studio 4.10 requires dwarf-2.

#     AVR [Extended] COFF format requires stabs, plus an avr-objcopy run.

DEBUG =

# Compiler flag to set the C Standard level.

#     c89   = "ANSI" C

#     gnu89 = c89 plus GCC extensions

#     c99   = ISO C99 standard (not yet fully implemented)

#     gnu99 = c99 plus GCC extensions

CSTANDARD = -std=gnu99

# Place -D or -U options here

CDEFS += -DF_CPU=$(F_CPU)UL

CDEFS += -DFFP

# for wireless library

ifdef USE_WIRELESS

	CDEFS += -DROBOT

endif

# Place -I, -L options here

CINCS = -I$(COLONYROOT)/code/lib/include/libdragonfly

CINCS += -L$(COLONYROOT)/code/lib/bin

ifdef USE_WIRELESS

	CINCS += -I$(COLONYROOT)/code/lib/include/libwireless

endif

#---------------- Compiler Options ----------------

#  -g*:          generate debugging information

#  -O*:          optimization level

#  -f...:        tuning, see GCC manual and avr-libc documentation

#  -Wall...:     warning level

#  -Wa,...:      tell GCC to pass this to the assembler.

#    -adhlns...: create assembler listing

CFLAGS =

# CFLAGS = -g$(DEBUG)

CFLAGS += $(CDEFS) $(CINCS)

CFLAGS += -O$(OPT)

CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums

CFLAGS += -Wall -Wstrict-prototypes

CFLAGS += -Wa,-adhlns=$(<:.c=.lst)

CFLAGS += $(CSTANDARD)

#---------------- Assembler Options ----------------

#  -Wa,...:   tell GCC to pass this to the assembler.

#  -ahlms:    create listing

#  -gstabs:   have the assembler create line number information; note that

#             for use in COFF files, additional information about filenames

#             and function names needs to be present in the assembler source

#             files -- see avr-libc docs [FIXME: not yet described there]

ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs

#---------------- Library Options ----------------

# Minimalistic printf version

PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min

# Floating point printf version (requires MATH_LIB = -lm below)

PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt

# If this is left blank, then it will use the Standard printf version.

#PRINTF_LIB =

#PRINTF_LIB = $(PRINTF_LIB_MIN)

PRINTF_LIB = $(PRINTF_LIB_FLOAT)

# Minimalistic scanf version

SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min

# Floating point + %[ scanf version (requires MATH_LIB = -lm below)

SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt

# If this is left blank, then it will use the Standard scanf version.

SCANF_LIB =

#SCANF_LIB = $(SCANF_LIB_MIN)

#SCANF_LIB = $(SCANF_LIB_FLOAT)

MATH_LIB = -lm

#---------------- External Memory Options ----------------

# 64 KB of external RAM, starting after internal RAM (ATmega128!),

# used for variables (.data/.bss) and heap (malloc()).

#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff

# 64 KB of external RAM, starting after internal RAM (ATmega128!),

# only used for heap (malloc()).

#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff

EXTMEMOPTS =

#---------------- Linker Options ----------------

#  -Wl,...:     tell GCC to pass this to linker.

#    -Map:      create map file

#    --cref:    add cross reference to  map file

LDFLAGS = -Wl,-Map=$(TARGET).map,--cref

LDFLAGS += $(EXTMEMOPTS)

LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB)

ifdef USE_WIRELESS

	LDFLAGS += -lwireless

endif

LDFLAGS += -ldragonfly

#---------------- Programming Options (avrdude) ----------------

# Programming hardware: alf avr910 avrisp bascom bsd

# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500

#

# Type: avrdude -c ?

# to get a full listing.

#

AVRDUDE_PROGRAMMER = avrisp

# programmer connected to serial device

AVRDUDE_WRITE_FLASH = -b 57600 -U flash:w:$(TARGET).hex

#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep

# Uncomment the following if you want avrdude's erase cycle counter.

# Note that this counter needs to be initialized first using -Yn,

# see avrdude manual.

#AVRDUDE_ERASE_COUNTER = -y

# Uncomment the following if you do /not/ wish a verification to be

# performed after programming the device.

#AVRDUDE_NO_VERIFY = -V

# Increase verbosity level.  Please use this when submitting bug

# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude>

# to submit bug reports.

#AVRDUDE_VERBOSE = -v -v

AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)

AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY)

AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE)

AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER)

#don't check for device signature

AVRDUDE_FLAGS += -F

#---------------- Debugging Options ----------------

# For simulavr only - target MCU frequency.

DEBUG_MFREQ = $(F_CPU)

# Set the DEBUG_UI to either gdb or insight.

# DEBUG_UI = gdb

DEBUG_UI = insight

# Set the debugging back-end to either avarice, simulavr.

DEBUG_BACKEND = avarice

#DEBUG_BACKEND = simulavr

# GDB Init Filename.

GDBINIT_FILE = __avr_gdbinit

# When using avarice settings for the JTAG

JTAG_DEV = /dev/com1

# Debugging port used to communicate between GDB / avarice / simulavr.

DEBUG_PORT = 4242

# Debugging host used to communicate between GDB / avarice / simulavr, normally

#     just set to localhost unless doing some sort of crazy debugging when

#     avarice is running on a different computer.

DEBUG_HOST = localhost

#============================================================================

# Define programs and commands.

SHELL = sh

CC = avr-gcc

OBJCOPY = avr-objcopy

OBJDUMP = avr-objdump

SIZE = avr-size

NM = avr-nm

AVRDUDE = avrdude

REMOVE = rm -f

REMOVEDIR = rm -rf

COPY = cp

WINSHELL = cmd

# Define Messages

# English

MSG_ERRORS_NONE = Errors: none

MSG_BEGIN = -------- begin --------

MSG_END = --------  end  --------

MSG_SIZE_BEFORE = Size before:

MSG_SIZE_AFTER = Size after:

MSG_COFF = Converting to AVR COFF:

MSG_EXTENDED_COFF = Converting to AVR Extended COFF:

MSG_FLASH = Creating load file for Flash:

MSG_EEPROM = Creating load file for EEPROM:

MSG_EXTENDED_LISTING = Creating Extended Listing:

MSG_SYMBOL_TABLE = Creating Symbol Table:

MSG_LINKING = Linking:

MSG_COMPILING = Compiling:

MSG_ASSEMBLING = Assembling:

MSG_CLEANING = Cleaning project:

# Define all object files.

OBJ = $(SRC:.c=.o) $(ASRC:.S=.o)

# Define all listing files.

LST = $(SRC:.c=.lst) $(ASRC:.S=.lst)

# Compiler flags to generate dependency files.

GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d

# Combine all necessary flags and optional flags.

# Add target processor to flags.

ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS)

ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)

# Default target.

all: begin gccversion sizebefore build sizeafter end

build: elf hex eep lss sym

elf: $(TARGET).elf

hex: $(TARGET).hex

eep: $(TARGET).eep

lss: $(TARGET).lss

sym: $(TARGET).sym

# Eye candy.

# AVR Studio 3.x does not check make's exit code but relies on

# the following magic strings to be generated by the compile job.

begin:

	@echo

	@echo $(MSG_BEGIN)

end:

	@echo $(MSG_END)

	@echo

# Display size of file.

HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex

ELFSIZE = $(SIZE) -A $(TARGET).elf

AVRMEM = avr-mem.sh $(TARGET).elf $(MCU)

sizebefore:

	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \

	$(AVRMEM) 2>/dev/null; echo; fi

sizeafter:

	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \

	$(AVRMEM) 2>/dev/null; echo; fi

# Display compiler version information.

gccversion :

	@$(CC) --version

# Program the device.

program: $(TARGET).hex $(TARGET).eep

	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)

# Generate avr-gdb config/init file which does the following:

#     define the reset signal, load the target file, connect to target, and set

#     a breakpoint at main().

gdb-config:

	@$(REMOVE) $(GDBINIT_FILE)

	@echo define reset >> $(GDBINIT_FILE)

	@echo SIGNAL SIGHUP >> $(GDBINIT_FILE)

	@echo end >> $(GDBINIT_FILE)

	@echo file $(TARGET).elf >> $(GDBINIT_FILE)

	@echo target remote $(DEBUG_HOST):$(DEBUG_PORT)  >> $(GDBINIT_FILE)

ifeq ($(DEBUG_BACKEND),simulavr)

	@echo load  >> $(GDBINIT_FILE)

endif

	@echo break main >> $(GDBINIT_FILE)

debug: gdb-config $(TARGET).elf

ifeq ($(DEBUG_BACKEND), avarice)

	@echo Starting AVaRICE - Press enter when "waiting to connect" message displays.

	@$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \

	$(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT)

	@$(WINSHELL) /c pause

else

	@$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \

	$(DEBUG_MFREQ) --port $(DEBUG_PORT)

endif

	@$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE)

# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB.

COFFCONVERT=$(OBJCOPY) --debugging \

--change-section-address .data-0x800000 \

--change-section-address .bss-0x800000 \

--change-section-address .noinit-0x800000 \

--change-section-address .eeprom-0x810000

coff: $(TARGET).elf

	@echo

	@echo $(MSG_COFF) $(TARGET).cof

	$(COFFCONVERT) -O coff-avr $< $(TARGET).cof

extcoff: $(TARGET).elf

	@echo

	@echo $(MSG_EXTENDED_COFF) $(TARGET).cof

	$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof

# Create final output files (.hex, .eep) from ELF output file.

%.hex: %.elf

	@echo

	@echo $(MSG_FLASH) $@

	$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@

%.eep: %.elf

	@echo

	@echo $(MSG_EEPROM) $@

	-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \

	--change-section-lma .eeprom=0 -O $(FORMAT) $< $@

# Create extended listing file from ELF output file.

%.lss: %.elf

	@echo

	@echo $(MSG_EXTENDED_LISTING) $@

	$(OBJDUMP) -h -S $< > $@

# Create a symbol table from ELF output file.

%.sym: %.elf

	@echo

	@echo $(MSG_SYMBOL_TABLE) $@

	$(NM) -n $< > $@

# Link: create ELF output file from object files.

.SECONDARY : $(TARGET).elf

.PRECIOUS : $(OBJ)

%.elf: $(OBJ)

	@echo

	@echo $(MSG_LINKING) $@

	$(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS)

# Compile: create object files from C source files.

%.o : %.c

	@echo

	@echo $(MSG_COMPILING) $<

	$(CC) -c $(ALL_CFLAGS) $< -o $@

# Compile: create assembler files from C source files.

%.s : %.c

	$(CC) -S $(ALL_CFLAGS) $< -o $@

# Assemble: create object files from assembler source files.

%.o : %.S

	@echo

	@echo $(MSG_ASSEMBLING) $<

	$(CC) -c $(ALL_ASFLAGS) $< -o $@

# Create preprocessed source for use in sending a bug report.

%.i : %.c

	$(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@

# Target: clean project.

clean: begin clean_list end

clean_list :

	@echo

	@echo $(MSG_CLEANING)

	$(REMOVE) $(TARGET).hex

	$(REMOVE) $(TARGET).eep

	$(REMOVE) $(TARGET).cof

	$(REMOVE) $(TARGET).elf

	$(REMOVE) $(TARGET).map

	$(REMOVE) $(TARGET).sym

	$(REMOVE) $(TARGET).lss

	$(REMOVE) $(OBJ)

	$(REMOVE) $(LST)

	$(REMOVE) $(SRC:.c=.s)

	$(REMOVE) $(SRC:.c=.d)

	$(REMOVEDIR) .dep

# Include the dependency files.

-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)

# Listing of phony targets.

.PHONY : all begin finish end sizebefore sizeafter gccversion \

build elf hex eep lss sym coff extcoff \

clean clean_list program debug gdb-config

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

#define CHANNEL 0xF // channel for wireless communication

#define TYPE 42	// packet type for wireless communication

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

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

    // set up wireless

    wl_basic_init_default();

    wl_set_channel(CHANNEL);

    wl_set_com_port("/dev/ttyUSB0");

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

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

	wl_basic_send_global_packet(TYPE, send_buffer, 2);

	printf("PASSED\n");

    }

    else

	printf("FAILED\n");

    // 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

    usleep(800000);  // wait for 800 ms before sending ACK

    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\n");

    else

	printf("PASSED\n");

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

    {

	// wait before sending ACK back

	usleep(900000);

	if (wl_basic_do_default(&data_length))

	    printf("FAILED\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");

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

    {

	usleep(5000000);    // wait 5 seconds to see if they TAG again

	if (wl_basic_do_default(&data_length))

	    printf("FAILED\n");

	else

	    printf("PASSED\n");

    }

    else

	printf("FAILED\n");

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

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

    return 0;

}

#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

 */

uint8_t 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>

/*

 * 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 <wireless.h>

#include <wl_basic.h>

#include "smart_run_around_fsm.h"

#include "hunter_prey.h"

#define CHAN 0xF

#define TAG_PAUSE 2000

#define ROBOT_HUNTER 0

#define ROBOT_PREY 1

#define ROBOT_TAGGED 2

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

volatile uint8_t robotState = ROBOT_HUNTER;

volatile uint8_t color1, color2;

volatile uint8_t tagAck;

volatile uint8_t tagger;

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

    usb_putc(' ');

    usb_puti(range_read_distance (IR5));

    usb_putc('\n');

    delay_ms(500);

  }

}

void chase(uint8_t angle) {

  int omega = angle - 4;

  if(angle > 16)

    return;

  if(omega >  8)

    omega = omega - 16;

  omega *= 255/8;

/*   usb_puti(omega); */

/*   usb_putc('\n'); */

  move(140, omega);

}

int main(void) {

  int frontRange;

  uint8_t angle;

  unsigned char packet[2];

  //intialize the robot and wireless

  dragonfly_init(ALL_ON);

  //  test_bom();

  id = get_robotid();

  packet[1] = id;

  usb_puts("hello, I am robot \n");

  usb_puti(id);

  usb_putc('\n');

  usb_puts("wireless.\n");

  wl_basic_init(&packet_receive);

  usb_puts("run around.\n");

  run_around_init();

  usb_puts("channel\n");

  //use a specific channel to avoid interfering with other tasks

  wl_set_channel(CHAN);

  usb_puts("orb set\n");

  orb_set_color(BLUE);

  color1 = BLUE;

  color2 = BLUE;

  //if button 2 is held, you are prey

  if(button2_read()) {

    robotState = ROBOT_PREY;

    bom_on();

  }

  while(1) {

    if(robotState==ROBOT_TAGGED) {

      usb_puts("tagged, waiting to send ACK\n");

      delay_ms(TAG_PAUSE);

      move(0,0);

      usb_puts("sending ACK to ");

      usb_puti(tagger);

      usb_putc('\n');

      packet[0] = HUNTER_PREY_ACTION_ACK;

      packet[1] = tagger;

      wl_basic_send_global_packet (0, &packet, 2);

      usb_puts("sent\n");

      robotState = ROBOT_HUNTER; //no longer prey

    }

    else if(robotState == ROBOT_PREY) {

      run_around_FSM();

    }

    else { //ROBOT_HUNTER

      bom_refresh(BOM_ALL);

      delay_ms(100);

      frontRange = range_read_distance (IR2);

/*       usb_puti(frontRange); */

/*       usb_putc(','); */

      //BUG: when get_max_bom is called, the rangefinder seems to stop

      //working

      angle = bom_get_max();

/*       usb_puti(angle); */

/*       usb_putc('\n'); */

      chase(angle);

      //debugging to determine tagging conditions

      if(angle < 7 && angle > 1) {

	if(color1 != RED) {

	  color1 = RED;

	  orb1_set_color(RED);

	}

      }

      else {

	if(color1 != BLUE) {

	  color1 = BLUE;

	  orb1_set_color(BLUE);

	}

      }

      usb_puti(frontRange);

      usb_putc('\n');

/*       if(frontRange > 0 && frontRange < TAG_RANGE) { */

/* 	if(color2 != RED) { */

/* 	  color2 = RED; */

/* 	  orb2_set_color(RED); */

/* 	} */

/*       } */

/*       else { */

/* 	if(color2 != BLUE) { */

/* 	  color2 = BLUE; */

/* 	  orb2_set_color(BLUE); */

/* 	} */

/*       } */

      if(hunter_prey_tagged(angle, frontRange)) {

	orb_set_color(YELLOW);

	color1 = YELLOW;

	color2 = YELLOW;

	usb_puts("TAG!\n");

	tagAck = 0;

	packet[0] = HUNTER_PREY_ACTION_TAG;

	packet[1] = id;

	wl_basic_send_global_packet (0, &packet, 2);

	move(0,0);

	while(!tagAck)

	  wl_do();

	if(tagAck == id) { //if the ack was for us

	  usb_puts("ACK!\n");

	  move(-230,0); //back up to give the new prey some room

	  delay_ms(TAG_PAUSE/2);

	  move(0,0);

	  robotState = ROBOT_PREY;

	  bom_on();

	}

	else {

	  usb_puts("ACK failed!\n");

	}

      }

    }

    wl_do();

  }

  return 0;

}

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

{

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

    if(packet[0] == HUNTER_PREY_ACTION_TAG) {

      if(robotState == ROBOT_PREY) {

	robotState = ROBOT_TAGGED;

	bom_off();

	color1 = BLUE;

	color2 = BLUE;

	orb_set_color(BLUE);

	tagger = packet[1];

      }

      else {

	usb_puts("tagged, but I'm not it!\n");

      }

    }

    else if(packet[0] == HUNTER_PREY_ACTION_ACK && robotState == ROBOT_HUNTER) {

      tagAck = packet[1];

    }

    else {

      usb_puts("got '");

      usb_putc(packet[0]);

      usb_puti(packet[1]);

      usb_puts("'\n");

    }

  }

  else {

    usb_puts("got unkown packet! type=");

    usb_puti(type);

    usb_puts(" length=");

    usb_puti(length);

    usb_putc('\n');

  }

}

#define HP_BUF_IDX_ACTION 0

#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

 */

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

}

#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(FULL_SPD, 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

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

/**

 * Tests the token ring and BOM

 * - prints table of bom values for every robot in token ring

 * - takes several seconds to initialize token ring

 */

#define MAX_ROBOTS 16

// set a list of integers to 0

void clearRobots(int* list) {

  for(int i=0;i<MAX_ROBOTS;i++)

    list[i] = 0;

}

int testtokenring(void) {

  usb_init();

	usb_puts("usb turned on\r\n");

  wl_init();

  usb_puts("wireless turned on\r\n");

  wl_token_ring_register();

  wl_token_ring_join(); // join token ring

  usb_puts("token ring joined\r\n");

  int* robotList = (int*)malloc(sizeof(int)*MAX_ROBOTS);

  int numRobots = 0;

  delay_ms(1000);

  // start testing wireless/token ring/BOM

  int i=0;

  while(1) {

    wl_do();

    // only print table every 200 loops

    if (i%200==0) {

      // get token ring size values

      usb_puts("\r\nnumber of robots in token ring:");

      usb_puti(wl_token_get_robots_in_ring());

      usb_puts("\r\nnumber of robots in matrix:"); 

      usb_puti(wl_token_get_num_robots());

      // get list of robots

      numRobots = 0;

      clearRobots(robotList);

      wl_token_iterator_begin();

      while(wl_token_iterator_has_next()) {

        int tmp = wl_token_iterator_next();

        if (tmp < 0)

          break;

        robotList[tmp] = 1;

        numRobots++;

      }

      if (numRobots < 1) {

        usb_puts("\r\nNo BOM table available.");

        continue; // skip table printing

      } else {

        usb_puts("\r\nBOM table: (* indicates this robot)");

      }

      // print table of bom readings between robots

      usb_puts("\r\ns \\ d");

      // print header

      for(int j=0;j<MAX_ROBOTS;j++)

        if (robotList[j]) {

          usb_puts("\t|");

          if (j == wl_get_xbee_id())

            usb_puts("*"); // indicate that this is the current bot

          usb_puti(j);

        }

      usb_puts("\r\n");

      // print body

      for(int l=0;l<MAX_ROBOTS;l++) 

        if (robotList[l]) {        

          if (l == wl_get_xbee_id())

            usb_puts("*"); // indicate that this is the current bot

          usb_puti(l); // print label col

          for(int k=0;k<MAX_ROBOTS;k++) 

            if (robotList[k]) {

              usb_puts("\t|");

              if (k != l) {

                int bom = wl_token_get_sensor_reading(l,k);

                if (bom >= 0 && bom <= 15)

                  usb_puti(bom); // print bom value

              }

            }

          usb_puts("\r\n");

        }

      usb_puts("\r\n");

    }

  }

  // end testing token ring and bom

	return 0;

}

#include <dragonfly_lib.h>

/*

 * This function outputs to TeraTerm a table which displays the values currently

 * stored in the analog table

 */

int testanalog(void) {

	usb_init();

	usb_puts("usb turned on\r\n");

	range_init();

	usb_puts("rangefinders initialized\r\n");

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

	delay_ms(1000);

	while (1) {

		for (int i = 0; i < 20; i++) {

			usb_puts("\r\n");

		}

		usb_puts("port# |\tvalue\r\n");

		for (int j = 1; j < 16; j++) {

			usb_puti(j);

			usb_puts("\t");

			usb_puti(analog10(j));

			usb_puts("\r\n");

		}

		delay_ms(575);

	}

	return 0;

}

/**

 * @file encoders unit test

 */

#include <dragonfly_lib.h>

/**

 * @brief Test encoders by outputting values to usb

 *

 * @test  Tester should start a program to read data from the robot via usb

 *        (e.g. gtkterm). Turn on the robot on then turn each wheel manually.

 *        Make sure the output on the screen makes sense.

 *        Pressing button 1 resets total distance and time count

 * @pre   Depends on serial and dio to work correctly

 */

int testencoders(void)

{

	int encoder_left,encoder_right;

	int x_left, x_right;

	int v_left, v_right;

	int tc;

	while(1) {

		/* button1 is pressed */

		if (button1_read()) {

			/* reset dx and tc */

			encoder_rst_dx(LEFT);

			encoder_rst_dx(RIGHT);

			encoder_rst_tc();

		}

		encoder_left = encoder_read(LEFT);

		encoder_right = encoder_read(RIGHT);

		usb_puts("Encoder values (left, right): ");

		usb_puti(encoder_left); 

		usb_puts(", ");

		usb_puti(encoder_right); 

		usb_puts("\n");

		x_left = encoder_get_x(LEFT);