Colony

#include <stdlib.h>

#include <stdio.h>

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

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

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

    }

    WINDOW* win = initscr();

    nodelay(win, TRUE);

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

        wl_set_com_port("/dev/ttyUSB1");

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

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

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

}

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

    if (type == POINT_RAW) {

        //expected input: enc_l enc_r 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");

    }

    else if (type == POINT_ODO) {

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

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

        //double theta;

        float theta;

        long theta_temp;

        x = (char)packet[0] + ((char)packet[1] << 8);

        y = (char)packet[2] + ((char)packet[3] << 8);

        theta_temp = (char)packet[4] + ((char)packet[5] << 8)

            + ((char)packet[5] << 16) + ((char)packet[6] << 24);

        theta = (float)theta_temp;

        ir1 = (char)packet[8] + ((char)packet[9] << 8);

        ir2 = (char)packet[10] + ((char)packet[11] << 8);

        ir3 = (char)packet[12] + ((char)packet[13] << 8);

        ir4 = (char)packet[14] + ((char)packet[15] << 8);

        ir5 = (char)packet[16] + ((char)packet[17] << 8);

        fprintf(file, "%d %d %f %d %d %d %d %d\n", x, y, theta, ir1, ir2, ir3, ir4, ir5);

    }

}