Colony

#include <stdlib.h>

#include <stdio.h>

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

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

#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 color, 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 = {1, NULL, NULL, &packet_receive, NULL};

FILE *file;

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

    if (argc != 2) {

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

        return 1;

    }

    char c;

    int robot = atoi(argv[1]);

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

    if (file == NULL) {

        printf("fopen error\n");

        return 1;

    }

    initscr();

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

        wl_set_com_port("/dev/ttyUSB0");

        wl_init();

        wl_set_channel(0xF);

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

        wl_register_packet_group(&cntlHandler);

        wl_register_packet_group(&receiveHandler);

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

    fflush(stdout);

        //while (1);

        while (1) {

        wl_do();

        c = getch();

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

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

    //wl_send_global_packet(WL_CNTL_GROUP, type, NULL, 0, 0);

}

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

    //expected input: x y theta IR1 IR2 IR3 IR4 IR5

    int i, temp;

    for (i = 0; i < length; i += 2) {

        temp = (char)packet[i+1] << 8;

        temp |= (char)packet[i];

        fprintf(file, "%d ", temp);

    }

    fprintf(file, "\n");

}