Colony

#include <stdlib.h>

#include <stdio.h>

#include <string.h>

#include <curses.h> // you need to install the ncurses library

#include "../../libwireless/lib/wireless.h"

#include <unistd.h>

#define CHAN 0xE

#define MAP 1   // packet group for receiving 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

void send_packet (char type, int dst_robot);

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

PacketGroupHandler cntlHandler = {WL_CNTL_GROUP, NULL, NULL, NULL, NULL};

PacketGroupHandler receiveHandler = {MAP, NULL, NULL, &packet_receive, NULL};

FILE *file;

int parent_running = 1;

int main(int argc, char *argv[]) {

    if (argc != 3) {

        printf("Usage: ./test <robot #> <USB port #>\n");

        return 1;

    }

    char c;

    int robot = atoi(argv[1]);

    char port[14];        /* port that XBee is on */

    /* USB port to use is second cmd-line argument */

    strcpy(port, "/dev/ttyUSB");

    strcat(port, argv[2]);

    printf("Beginning: robot=%d\r\n", robot);

    wl_set_com_port(port);

    wl_init();

    wl_set_channel(CHAN);

    printf("Wireless initialized.\r\n");

    wl_register_packet_group(&cntlHandler);

    wl_register_packet_group(&receiveHandler);

    printf("Packet groups initialized.\r\n");

    fflush(stdout);

    file = fopen("input.txt", "w");

    if (file == NULL) {

        printf("fopen error\n");

        return 1;

    }

    WINDOW* win = initscr();

    nodelay(win, TRUE);

    while (1) {

        wl_do();

        c = getch();

        if(c != ERR) {

            printf("%c\r\n", c);

            fflush(stdout);

            if (c=='0')

                break;

            switch (c) {

                case 'w':

                    send_packet(CNTL_FORWARD, robot);

                    break;

                case 's':

                    send_packet(CNTL_BACK, robot);

                    break;

                case 'a':

                    send_packet(CNTL_LEFT, robot);

                    break;

                case 'd':

                    send_packet(CNTL_RIGHT, robot);

                    break;

                case 'q':

                    send_packet(CNTL_LEFT_CURVE, robot);

                    break;

                case 'e':

                    send_packet(CNTL_RIGHT_CURVE, robot);

                    break;

                case 'x':

                    send_packet(CNTL_STOP, robot);

                    break;

                case ']':

                    send_packet(CNTL_VEL_UP, robot);

                    break;

                case '[':

                    send_packet(CNTL_VEL_DOWN, robot);

                    break;

                default:

                    //send_packet(CNTL_STOP, robot);

                    break;

            }

        }

    }

    fclose(file);

    delwin(win);

    endwin();

    refresh();

    return 0;

}

void send_packet (char type, int dst_robot) {

    wl_send_robot_to_robot_global_packet(WL_CNTL_GROUP, type,

            NULL, 0, dst_robot, 0);

}

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

    short x,y,ir1,ir2,ir3,ir4,ir5;

    /* convert received data from 2 chars to 1 short */

    x = ((short)packet[1] << 8) | (short)packet[0];

    y = ((short)packet[3] << 8) | (short)packet[2];

    ir1 = ((short)packet[9] << 8) | (short)packet[8];

    ir2 = ((short)packet[11] << 8) | (short)packet[10];

    ir3 = ((short)packet[13] << 8) | (short)packet[12];

    ir4 = ((short)packet[15] << 8) | (short)packet[14];

    ir5 = ((short)packet[17] << 8) | (short)packet[16];

    char tarr[] = {packet[4],packet[5],packet[6],packet[7]};

    float *theta_ptr = (float *)tarr;

    float theta = *theta_ptr;

    fprintf(file, "%d %d %f %d %d %d %d %d\n", x, y, theta,

            ir1, ir2, ir3, ir4, ir5);

}