root / branches / colonetmk2 / code / projects / swarm / main.c @ 1456
History | View | Annotate | Download (4.13 KB)
1 | 1456 | rcahoon | #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 | } |