Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_basic.h>

#include "hunter_prey.h"

/* Command Types */

#define CNTL_FORWARD 'f'

#define CNTL_STOP    's'

#define CNTL_BACK    'b'

#define CNTL_LEFT    'l'

#define CNTL_RIGHT   'r'

#define CNTL_BUZZ0   't'

#define CNTL_BUZZ1   'u'

#define WL_CNTL_GROUP 4

#define HUNTER 1

#define HUNTER_TAGGING 5

#define WAIT 2

#define PREY 3

#define DO_TAG 4

#define WL_CHANNEL 24

#define WAIT_DELAY_MS 2000

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

PacketGroupHandler colonetPacketHandler = 

{

    WL_CNTL_GROUP,

    NULL,

    NULL,

    &colonet_receive,

    NULL

};

int state;

unsigned char* packet_data;

int data_length;

int main (void) {

    char send_buffer[2];

    int robotID;

    int curTime = 0, oldTime = 0;

    dragonfly_init(ALL_ON);

    xbee_init();

    wl_basic_init_default();

    wl_set_channel(WL_CHANNEL);

    // register handler for colonet stuff

    wl_register_packet_group(&colonetPacketHandler);

    robotID = get_robotid();

    usb_puts("robot ");

    usb_puti(robotID);

    usb_puts("entering main loop\n");

    state = HUNTER;

    while (1) {

        switch(state) {

        case HUNTER_TAGGING:

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

                    usb_puts("got ACK, going to PREY\n");

                    bom_on();

                    state = PREY;

                } else {

                    /* If we get an ACK with a different robotID, then someone beat us

                     * to the TAG, so we must wait */

                    usb_puts("someone else got ACK, going to WAIT\n");

                    state = WAIT;

                }

            }

            else if (curTime - oldTime > 1) {

                /* If 1 second has ellapsed, return to normal hunting state (we can

                 * TAG again now) */

                usb_puts("giving up on tag.. going to HUNTER\n");

                state = HUNTER;

            }

            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;

            }

            break;

        case HUNTER:

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

                usb_puts("going to WAIT\n");

                state = WAIT;

            }

            break;

        case PREY:

            orbs_set_color(GREEN, GREEN);

            if (packet_data && data_length == 2

                && packet_data[0] == HUNTER_PREY_ACTION_ACK) {

                /* If we've received an ACK, we need to wait */

                usb_puts("SOMEONE ACK'd, going to WAIT\n");

                bom_off();

                state = WAIT;

            }

            else if (packet_data && data_length == 2

                     && packet_data[0] == HUNTER_PREY_ACTION_TAG) {

                /* Check if we've received a TAG. If so then send an

                 * ACK back */

                usb_puts("got TAG, sending ACK\n");

                send_buffer[0] = HUNTER_PREY_ACTION_ACK;

                send_buffer[1] = packet_data[1];

                wl_basic_send_global_packet(42, send_buffer, 2);

                bom_off();

                state = WAIT;

            }

            break;

        case WAIT:

            motor1_set(0,0);

            motor2_set(0,0);

            orbs_set_color(BLUE, BLUE);

            delay_ms(WAIT_DELAY_MS);

            state = HUNTER;

            break;

        case DO_TAG:

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

                usb_puts("someone beat us to TAG, going to WAIT\n");

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

                usb_puts("sent TAG, going to HUNTER_TAGGING\n");

                oldTime = rtc_get();

                state = HUNTER_TAGGING;

            }

            break;

        default:

            state = HUNTER;

            break;

        }

        packet_data = wl_basic_do_default(&data_length);

            delay_ms(50);

    }

}

/**

 *

 * This packet handler accepts colonet packets from discrete_control

 * in the colonet/robot/joystick folder 

 */

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

    int times;

    int frontIR = 0;

    int maxBOM = 0;

    static int last = 0;

    /* if we are in WAIT, don't move or tag */

    if(state == WAIT)

        return;

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

    /* // If the robot is changing direction, stop it for a bit first */

    /* if (last != type) { */

    /*         if( (last == CNTL_FORWARD || last == CNTL_BACK || last == CNTL_LEFT || last == CNTL_RIGHT) && */

    /*             (type == CNTL_FORWARD || type == CNTL_BACK || type == CNTL_LEFT || type == CNTL_RIGHT)) { */

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

    /*             motor1_set(0,0); */

    /*             motor2_set(0,0); */

    /*             delay_ms(100); */

    /*         } */

    /* } */

    /* last = type; */

    switch (type) {

    case CNTL_FORWARD:

        motor1_set(1, 250);

        motor2_set(1, 250);

        break;

    case CNTL_STOP:

        motor1_set(0, 0);

        motor2_set(0, 0);

        delay_ms(220); // 100:200

        break;

    case CNTL_BACK:

        motor1_set(0, 250);

        motor2_set(0, 250);

        break;

    case CNTL_LEFT:

        motor1_set(0, 250);

        motor2_set(1, 250);

        break;

    case CNTL_RIGHT:

        motor1_set(1, 250);

        motor2_set(0, 250);

        break;

    /* front trigger */

    case CNTL_BUZZ0: 

        if (state == HUNTER) {

            usb_puts("trigger\n");

            buzzer_chirp(50, 440);

            /* try to tag 10 times in a row */

            /* NOTE: this needs to be at least TAG_TIME times (from

             * hunter_prey.c */

            for(times = 0; times < 10; times++) {

                usb_puti(times);

                bom_refresh(BOM_ALL);

                frontIR = range_read_distance(IR2);

                maxBOM = get_max_bom();

                usb_puts(": IR=");

                usb_puti(frontIR);

                usb_puts(" BOM=");

                usb_puti(maxBOM);

                usb_putc('\n');

                if (hunter_prey_tagged(maxBOM, frontIR)) {

                    usb_puts("tagged!\n");

                    buzzer_chirp(100, 660);

                    state = DO_TAG;

                    break;

                }

                else {

                    buzzer_chirp(10, 349);

                }

            }

        }

        break;

    default:

        motor1_set(0, 0);

        motor2_set(0, 0);

        break;

    }

}