Statistics
| Revision:

root / branches / colonetmk2 / code / projects / swarm / main.c @ 1456

History | View | Annotate | Download (4.13 KB)

1
#include <wireless.h>
2
#include <wl_token_ring.h>
3

    
4
#include <stdio.h>
5
#include <string.h>
6
#include <unistd.h>
7

    
8
#include <sys/socket.h>
9
#include <arpa/inet.h>
10

    
11
#include <sys/ioctl.h>
12

    
13
typedef struct {
14
    int x;
15
    int y;
16
    char c;
17
} arrow_t;
18

    
19
arrow_t arrows[16] = {
20
                    { 8, 2, '-'  }, // 0
21
                    { 8, 1, '/'  }, // 1
22
                    { 7, 0, '/'  }, // 2
23
                    { 5, 0, '/'  }, // 3
24
                    { 4, 0, '|'  }, // 4
25
                    { 3, 0, '\\' }, // 5
26
                    { 1, 0, '\\' }, // 6
27
                    { 0, 1, '\\' }, // 7
28
                    { 0, 2, '-'  }, // 8
29
                    { 0, 3, '/'  }, // 9
30
                    { 1, 4, '/'  }, // 10
31
                    { 3, 4, '/' }, // 11
32
                    { 4, 4, '|'  }, // 12
33
                    { 5, 4, '\\'  }, // 13
34
                    { 7, 4, '\\'  }, // 14
35
                    { 8, 3, '\\'  }, // 15
36
                  };
37
                    
38

    
39
char mobot[5][10] = {"   ___   ",
40
                     "  /   \\  ",
41
                     " |     | ",
42
                     "  \\___/  ",
43
                     "         "};
44
char mobot_m[5][10];
45

    
46
int main(int argc, char *argv[])
47
{
48
    int robots[16];
49
    SensorReading * sensors[16][16];
50
    int i,j;
51
        char sbuf[32];
52

    
53
        int sockfd, new_fd;  // listen on sock_fd, new connection on new_fd
54
        struct sockaddr_in serv_addr, their_addr;
55
        int rv;
56

    
57
    /*for(i=0; i<16; i++)
58
    {
59
        memcpy(mobot_m, mobot, 50*sizeof(char));
60
        arrow_t *arrow = &arrows[i];
61
        mobot_m[arrow->y][arrow->x] = arrow->c;
62
        printf("%d\n", i);
63
        for(j=0; j<5; j++)
64
        {
65
            printf("%s\n", mobot_m[j]);
66
        }
67
    }
68
    return 0;*/
69

    
70
    wl_set_com_port(argv[1]);
71

    
72
    if (wl_init() != 0) {
73
      fprintf(stderr, "wl_init failed.\n");
74
      return -1;
75
    }
76

    
77
    wl_set_channel(0xF);
78

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

    
81
    wl_token_ring_register();
82
    wl_token_ring_join();
83

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

    
86
        memset(&serv_addr, 0, sizeof serv_addr);
87
        serv_addr.sin_family = AF_INET;
88
        serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
89
        serv_addr.sin_port = htons(2000);
90

    
91
        if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
92
                perror("server: socket");
93
                return 1;
94
        }
95

    
96
        if ((rv = bind(sockfd, (struct sockaddr *)&serv_addr, sizeof serv_addr)) < 0)  {
97
                perror("bind");
98
                close(sockfd);
99
                return 2;
100
        }
101

    
102
        if (listen(sockfd, 0) == -1) {
103
                perror("listen");
104
                return 1;
105
        }
106

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

    
109
        while(1) {  // main accept() loop
110
                socklen_t sin_size = sizeof their_addr;
111

    
112
                wl_do();
113

    
114
                new_fd = accept(sockfd, (struct sockaddr *)&their_addr, &sin_size);
115
                if (new_fd == -1) {
116
                        perror("accept");
117
                        continue;
118
                }
119

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

    
122
                while (1)
123
                {
124
                    int num_robots = 0;
125

    
126
                    wl_do();
127

    
128
                    wl_token_iterator_begin();
129
                    while(wl_token_iterator_has_next())
130
                    {
131
                        robots[num_robots++] = wl_token_iterator_next();
132
                    }
133
                    for(i=0; i < num_robots; i++)
134
                    {
135
                        if (robots[i]==wl_get_xbee_id()) continue;
136
                        memcpy(mobot_m, mobot, 50*sizeof(char));
137
                        for(j=0; j < num_robots; j++)
138
                        {
139
                            if (i==j) continue;
140
                            //if (robots[j]==wl_get_xbee_id()) continue;
141
                            sensors[robots[i]][robots[j]] = wl_token_get_sensor_reading(robots[i], robots[j]);
142
                            printf("%d -> %d:  %d @ %d\n", robots[i], robots[j], sensors[robots[i]][robots[j]]->dir, sensors[robots[i]][robots[j]]->dist);
143
                            //if (sensors[robots[i]][robots[j]]->dir>160) continue;
144
                                        sprintf(sbuf, "st %d %d %d %d\n", robots[i], robots[j], sensors[robots[i]][robots[j]]->dir, sensors[robots[i]][robots[j]]->dist);
145
                                        if (send(new_fd, sbuf, strlen(sbuf), MSG_NOSIGNAL) < 0)
146
                                        {
147
                                                fprintf(stderr, "Error sending data\n");
148
                                                goto close_sock;
149
                                        }
150
                            arrow_t *arrow = &arrows[sensors[robots[i]][robots[j]]->dir/10];
151
                            mobot_m[arrow->y][arrow->x] = arrow->c;
152
                        }
153
                        /*for(j=0; j<5; j++)
154
                        {
155
                            printf("%s\n", mobot_m[j]);
156
                        }
157
                        printf("\n");*/
158
                    }
159
                    printf("\n");
160

    
161
                    usleep(130000);
162
                }
163
close_sock:
164
                close(new_fd);
165

    
166
                break;
167
        }
168
}