Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <encoders.h>

#include "odometry.h"

#define POLLING_INTERVAL 50    //interval in ms to get sensor data

#define MAP 1        // packet group for sending data points

#define POINT_RAW 1 // packet type for map data points w/ raw encoder data

#define POINT_ODO 2 // packet type for map data points w/ odometry data

#define WL_CNTL_GROUP 4

#define CNTL_FORWARD 0

#define CNTL_BACK    1

#define CNTL_LEFT    2

#define CNTL_RIGHT   3

#define CNTL_LEFT_CURVE 4

#define CNTL_RIGHT_CURVE 5

#define CNTL_STOP    6

#define CNTL_VEL_UP  7

#define CNTL_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};

PacketGroupHandler love_handle = {MAP, NULL, NULL, NULL, NULL};

int main(void)

{

    /* initialize components and join the token ring */

    dragonfly_init(ALL_ON);

    //range_init();

    //encoders_init();

    odometry_init();

    wl_init();

    wl_set_channel(0xF);

    wl_register_packet_group(&love_handle);

    wl_register_packet_group(&packetHandler);

    orb_enable();

    //data_response_init();

    //wl_token_ring_register();

    //wl_token_ring_join();

    //run_around_init();

    //usb_puti(xbee_get_address());

    //return 0;

    velocity = 160;

    state = CNTL_STOP;

    int x, y; // use when sending odometry data

    double theta;

    int store[9];

    char *send;   // for transmission

    while(1)

    {

        wl_do();

        //run_around_FSM();        /* traverse the environment and avoid obstacles ... */

        delay_ms(POLLING_INTERVAL);        /* until it's time to grab map data */

        x = odometry_dx(); // use when sending odometry data

        y = odometry_dy();

        theta = odometry_angle();

        //store[0] = encoder_get_dx(LEFT);    // left encoder distance

        //store[1] = encoder_get_dx(RIGHT);   // right encoder distance

        store[0] = x;

    store[1] = y;

    *((double*)(store + 2)) = theta;

    store[4] = range_read_distance(IR1);        // IR1 range

        store[5] = range_read_distance(IR2);        // IR2 range

        store[6] = range_read_distance(IR3);        // IR3 range

        store[7] = range_read_distance(IR4);        // IR4 range

        store[8] = range_read_distance(IR5);        // IR5 range

    sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],

          *((double *)(store+2)), store[4], store[5], store[6],

          store[7], store[8] );

    usb_puts(buf);

        send = (char*) store;

        // send point w/ raw encoder data

        // wl_send_global_packet(MAP, POINT_RAW, send, 14, 0);

        // send point w/ odometry data

        wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);

    }

    wl_terminate();

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

            state = CNTL_FORWARD;

            break;

        case CNTL_BACK:

            state = CNTL_BACK;

            break;

        case CNTL_LEFT:

            state = CNTL_LEFT;

            break;

        case CNTL_RIGHT:

            state = CNTL_RIGHT;

            break;

        case CNTL_LEFT_CURVE:

            state = CNTL_LEFT_CURVE;

            break;

        case CNTL_RIGHT_CURVE:

            state = CNTL_RIGHT_CURVE;

            break;

        case CNTL_STOP:

            state = CNTL_STOP;

            break;

        case CNTL_VEL_UP:

            if (velocity > 250)

                velocity = 250;

            else

                velocity += 5;

            break;

        case CNTL_VEL_DOWN:

            velocity -= 5;

            if (velocity < 150)

                velocity = 150;

            break;

        default:

            state = CNTL_STOP;

            break;

    }

    set_motion();

}

void set_motion (void) {

    switch (state) {

        case CNTL_FORWARD:

            orb_set_color(GREEN);

            move(velocity, 0);

            break;

        case CNTL_BACK:

            orb_set_color(BLUE);

            move(-velocity, 0);

            break;

        case CNTL_LEFT:

            orb_set_color(ORANGE);

            motor1_set(0, velocity);

            motor2_set(1, velocity);

            break;

        case CNTL_RIGHT:

            orb_set_color(YELLOW);

            motor1_set(1, velocity);

            motor2_set(0, velocity);

            break;

        case CNTL_LEFT_CURVE:

            orb_set_color(MAGENTA);

            motor1_set(1, 150);

            motor2_set(1, velocity);

            break;

        case CNTL_RIGHT_CURVE:

            orb_set_color(CYAN);

            motor1_set(1, velocity);

            motor2_set(1, 150);

            break;

        case CNTL_STOP:

            orb_set_color(RED);

            move(0, 0);

            break;

    }

}