Colony

#include <stdlib.h>

#include <stdio.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 != 2) {

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

        return 1;

    }

    char c;

    int robot = atoi(argv[1]);

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

    wl_init();

        wl_set_com_port("/dev/ttyUSB1");

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

}