Colony

#include <wireless.h>

#include <wl_token_ring.h>

#include <stdio.h>

#include <string.h>

#include <unistd.h>

#include <sys/socket.h>

#include <arpa/inet.h>

#include <sys/ioctl.h>

typedef struct {

    int x;

    int y;

    char c;

} arrow_t;

arrow_t arrows[16] = {

                    { 8, 2, '-'  }, // 0

                    { 8, 1, '/'  }, // 1

                    { 7, 0, '/'  }, // 2

                    { 5, 0, '/'  }, // 3

                    { 4, 0, '|'  }, // 4

                    { 3, 0, '\\' }, // 5

                    { 1, 0, '\\' }, // 6

                    { 0, 1, '\\' }, // 7

                    { 0, 2, '-'  }, // 8

                    { 0, 3, '/'  }, // 9

                    { 1, 4, '/'  }, // 10

                    { 3, 4, '/' }, // 11

                    { 4, 4, '|'  }, // 12

                    { 5, 4, '\\'  }, // 13

                    { 7, 4, '\\'  }, // 14

                    { 8, 3, '\\'  }, // 15

                  };

char mobot[5][10] = {"   ___   ",

                     "  /   \\  ",

                     " |     | ",

                     "  \\___/  ",

                     "         "};

char mobot_m[5][10];

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

{

    int robots[16];

    SensorReading * sensors[16][16];

    int i,j;

        char sbuf[32];

        int sockfd, new_fd;  // listen on sock_fd, new connection on new_fd

        struct sockaddr_in serv_addr, their_addr;

        int rv;

    /*for(i=0; i<16; i++)

    {

        memcpy(mobot_m, mobot, 50*sizeof(char));

        arrow_t *arrow = &arrows[i];

        mobot_m[arrow->y][arrow->x] = arrow->c;

        printf("%d\n", i);

        for(j=0; j<5; j++)

        {

            printf("%s\n", mobot_m[j]);

        }

    }

    return 0;*/

    wl_set_com_port(argv[1]);

    if (wl_init() != 0) {

      fprintf(stderr, "wl_init failed.\n");

      return -1;

    }

    wl_set_channel(0xF);

    printf("wireless on\r\n");

    wl_token_ring_register();

    wl_token_ring_join();

    printf("wireless initialized\r\n");

        memset(&serv_addr, 0, sizeof serv_addr);

        serv_addr.sin_family = AF_INET;

        serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);

        serv_addr.sin_port = htons(2000);

        if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {

                perror("server: socket");

                return 1;

        }

        if ((rv = bind(sockfd, (struct sockaddr *)&serv_addr, sizeof serv_addr)) < 0)  {

                perror("bind");

                close(sockfd);

                return 2;

        }

        if (listen(sockfd, 0) == -1) {

                perror("listen");

                return 1;

        }

        printf("server: waiting for connections...\n");

        while(1) {  // main accept() loop

                socklen_t sin_size = sizeof their_addr;

                wl_do();

                new_fd = accept(sockfd, (struct sockaddr *)&their_addr, &sin_size);

                if (new_fd == -1) {

                        perror("accept");

                        continue;

                }

                printf("server: got connection\n");

                while (1)

                {

                    int num_robots = 0;

                    wl_do();

                    wl_token_iterator_begin();

                    while(wl_token_iterator_has_next())

                    {

                        robots[num_robots++] = wl_token_iterator_next();

                    }

                    for(i=0; i < num_robots; i++)

                    {

                        if (robots[i]==wl_get_xbee_id()) continue;

                        memcpy(mobot_m, mobot, 50*sizeof(char));

                        for(j=0; j < num_robots; j++)

                        {

                            if (i==j) continue;

                            //if (robots[j]==wl_get_xbee_id()) continue;

                            sensors[robots[i]][robots[j]] = wl_token_get_sensor_reading(robots[i], robots[j]);

                            printf("%d -> %d:  %d @ %d\n", robots[i], robots[j], sensors[robots[i]][robots[j]]->dir, sensors[robots[i]][robots[j]]->dist);

                            //if (sensors[robots[i]][robots[j]]->dir>160) continue;

                                        sprintf(sbuf, "st %d %d %d %d\n", robots[i], robots[j], sensors[robots[i]][robots[j]]->dir, sensors[robots[i]][robots[j]]->dist);

                                        if (send(new_fd, sbuf, strlen(sbuf), MSG_NOSIGNAL) < 0)

                                        {

                                                fprintf(stderr, "Error sending data\n");

                                                goto close_sock;

                                        }

                            arrow_t *arrow = &arrows[sensors[robots[i]][robots[j]]->dir/10];

                            mobot_m[arrow->y][arrow->x] = arrow->c;

                        }

                        /*for(j=0; j<5; j++)

                        {

                            printf("%s\n", mobot_m[j]);

                        }

                        printf("\n");*/

                    }

                    printf("\n");

                    usleep(130000);

                }

close_sock:

                close(new_fd);

                break;

        }

}