root / branches / colonetmk2 / code / projects / swarm / main.c @ 1605
History | View | Annotate | Download (4.14 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 |
|
169 |
return 0; |
170 |
} |