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_ODO 2

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

int main(void)

{

    /* initialize components and join the token ring */

    dragonfly_init(ALL_ON);

    odometry_init();

    orb_enable();

    wl_init();

    wl_set_channel(0xE);

    wl_register_packet_group(&packetHandler);

    velocity = 190;

    state = CNTL_STOP;

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

    double theta;

    int store[9];

    char *send;   // for transmission

    while(1)

    {

        wl_do();

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

        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;

    }

}