Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include "../../../common/colonet_defs.h"

#define WL_CNTL_GROUP 4

#define FORWARD 0

#define BACK    1

#define LEFT    2

#define RIGHT   3

#define LEFT_CURVE 4

#define RIGHT_CURVE 5

#define STOP    6

#define VEL_UP  7

#define VEL_DOWN 8

int state;

int velocity;

char buf[100];

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

void set_motion (void);

PacketGroupHandler packetHandler = {WL_CNTL_GROUP, NULL, NULL, &packet_receive, NULL};

int main (void) {

    dragonfly_init(ALL_ON);

    wl_init();

    wl_set_channel(0xF);

    wl_register_packet_group(&packetHandler);

    orb_enable();

    //orb_disable();

    //orb_set(255,255,255);

    orb_set_color(PURPLE);

    //delay_ms(1000);

    //while(1);

    int xb_addr = xbee_get_address();

    sprintf(buf, "beginning: xbee addr=%d\n", xb_addr);

    usb_puts(buf);

    velocity = 160;

    state = STOP;

    while (1) {

        wl_do();

    }

    return 0;

}

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

    sprintf(buf, "received packet: %d\n", type);

    usb_puts(buf);

    switch (type) {

        case FORWARD:

            state = FORWARD;

            break;

        case BACK:

            state = BACK;

            break;

        case LEFT:

            state = LEFT;

            break;

        case RIGHT:

            state = RIGHT;

            break;

        case LEFT_CURVE:

            state = LEFT_CURVE;

            break;

        case RIGHT_CURVE:

            state = RIGHT_CURVE;

            break;

        case STOP:

            state = STOP;

            break;

        case VEL_UP:

            if (velocity > 250)

                velocity = 250;

            else

                velocity += 5;

            break;

        case VEL_DOWN:

            velocity -= 5;

            if (velocity < 150)

                velocity = 150;

            break;

        default:

            state = STOP;

            break;

    }

    set_motion();

}

void set_motion (void) {

    switch (state) {

        case FORWARD:

            orb_set_color(GREEN);

            move(velocity, 0);

            break;

        case BACK:

            orb_set_color(BLUE);

            move(-velocity, 0);

            break;

        case LEFT:

            orb_set_color(ORANGE);

            motor1_set(0, velocity);

            motor2_set(1, velocity);

            break;

        case RIGHT:

            orb_set_color(YELLOW);

            motor1_set(1, velocity);

            motor2_set(0, velocity);

            break;

        case LEFT_CURVE:

            orb_set_color(MAGENTA);

            motor1_set(1, 150);

            motor2_set(1, velocity);

            break;

        case RIGHT_CURVE:

            orb_set_color(CYAN);

            motor1_set(1, velocity);

            motor2_set(1, 150);

            break;

        case STOP:

            orb_set_color(RED);

            move(0, 0);

            break;

    }

}