Revision 297
trying to get server to send back num robots, xbee ids, sensor matrix -- not working yet
trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.cpp | ||
---|---|---|
14 | 14 |
#include <fcntl.h> |
15 | 15 |
|
16 | 16 |
#include <wireless.h> // Colonet wireless library. |
17 |
#include <wl_token_ring.h> |
|
17 | 18 |
|
18 | 19 |
#include "colonet_defs.h" |
19 | 20 |
#include "colonet_wireless.h" |
... | ... | |
57 | 58 |
|
58 | 59 |
strncpy(wl_port, wl_port_, 40); |
59 | 60 |
|
60 |
// todo - initialize wireless port |
|
61 |
wl_token_ring_register(); |
|
62 |
wl_token_ring_join(); |
|
61 | 63 |
|
62 | 64 |
message_handler = message_handler_; |
63 | 65 |
} |
... | ... | |
77 | 79 |
return 0; |
78 | 80 |
} |
79 | 81 |
|
80 |
void colonet_wl_send(short client_source, short dest, |
|
81 |
ColonetMessageType msg_type, unsigned char msg_code, |
|
82 |
void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code, |
|
82 | 83 |
unsigned char* args) { |
83 |
printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", |
|
84 |
client_source, dest, msg_code); |
|
84 |
printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code); |
|
85 | 85 |
|
86 | 86 |
ColonetRobotServerPacket pkt; |
87 | 87 |
pkt.client_id = client_source; |
... | ... | |
94 | 94 |
if (dest == GLOBAL_DEST) { |
95 | 95 |
printf("sending to global dest\n"); |
96 | 96 |
|
97 |
wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, |
|
98 |
(char*)(&pkt), sizeof(ColonetRobotServerPacket), 0); |
|
97 |
wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), sizeof(ColonetRobotServerPacket), 0); |
|
99 | 98 |
} else { |
100 | 99 |
printf("sending to specific robot.\n"); |
101 |
wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, |
|
102 |
(char)msg_type, (char*)(&pkt), |
|
103 |
sizeof(ColonetRobotServerPacket), |
|
104 |
dest, |
|
105 |
COLONET_RESPONSE_PACKET_FRAME_ID); |
|
100 |
wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
|
101 |
sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID); |
|
106 | 102 |
} |
107 | 103 |
} |
108 | 104 |
|
105 |
int colonet_get_num_robots(void) { |
|
106 |
return wl_token_get_num_robots(); |
|
107 |
} |
|
108 |
|
|
109 |
int* colonet_get_xbee_ids(int* n) { |
|
110 |
int num_robots = wl_token_get_num_robots(); |
|
111 |
int* ids = (int*)malloc(num_robots * sizeof(int)); |
|
112 |
|
|
113 |
wl_token_iterator_begin(); |
|
114 |
|
|
115 |
int* p = ids; |
|
116 |
while (wl_token_iterator_has_next()) { |
|
117 |
*p = wl_token_iterator_next(); |
|
118 |
p++; |
|
119 |
} |
|
120 |
|
|
121 |
*n = num_robots; |
|
122 |
return ids; |
|
123 |
} |
|
124 |
|
|
125 |
// Returns int**; should be freed |
|
126 |
int** colonet_get_sensor_matrix(int* n) { |
|
127 |
int num_robots; |
|
128 |
int* ids = colonet_get_xbee_ids(&num_robots); |
|
129 |
|
|
130 |
int** m = (int**)malloc(num_robots * sizeof(int*)); |
|
131 |
for (int i = 0; i < num_robots; i++) { |
|
132 |
m[i] = (int*)malloc(num_robots * sizeof(int*)); |
|
133 |
} |
|
134 |
|
|
135 |
for (int i = 0; i < num_robots; i++) { |
|
136 |
for (int j = 0; j < num_robots; j++) { |
|
137 |
m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
|
138 |
} |
|
139 |
} |
|
140 |
|
|
141 |
free(ids); |
|
142 |
|
|
143 |
*n = num_robots; |
|
144 |
return m; |
|
145 |
} |
|
146 |
|
|
109 | 147 |
/**************************** Private functions ******************************/ |
110 | 148 |
|
111 | 149 |
static void timeout_handler() { |
112 |
printf("timeout!\n"); |
|
150 |
printf("colonet wireless - timeout!\n");
|
|
113 | 151 |
} |
114 | 152 |
|
115 | 153 |
static void handle_response(int frame, int received) { |
... | ... | |
172 | 210 |
* |
173 | 211 |
* @return 0 on success, -1 on failure |
174 | 212 |
*/ |
175 |
int log_packet(unsigned char* packet, int len) |
|
176 |
{ |
|
213 |
int log_packet(unsigned char* packet, int len) { |
|
177 | 214 |
FILE* logfile = fopen(log_filename, "a"); |
178 | 215 |
|
179 | 216 |
if (logfile == NULL) { |
trunk/code/projects/colonet/lib/colonet_wireless/colonet_wireless.h | ||
---|---|---|
42 | 42 |
* |
43 | 43 |
* @return void |
44 | 44 |
*/ |
45 |
void colonet_wl_send(short client_source, short dest, |
|
46 |
ColonetMessageType msg_type, unsigned char msg_code, |
|
45 |
void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code, |
|
47 | 46 |
unsigned char* args); |
48 | 47 |
|
48 |
int colonet_get_num_robots(void); |
|
49 |
|
|
50 |
int** colonet_get_sensor_matrix(int* n); |
|
51 |
int* colonet_get_xbee_ids(int* n); |
|
52 |
|
|
49 | 53 |
#endif |
trunk/code/projects/colonet/ColonetServer/Command.cpp | ||
---|---|---|
224 | 224 |
return -1; |
225 | 225 |
} |
226 | 226 |
|
227 |
//TODO: change after we start keeping track of bom matrix |
|
228 |
/* |
|
229 |
int number_robots = rand()%10; |
|
230 |
int bom_matrix[number_robots][number_robots]; |
|
231 |
for (int ii = 0; ii < number_robots; ii++) { |
|
232 |
for (int j = 0; j < number_robots; j++) { |
|
233 |
//do this to generate some -1 values which mean they can't see each other |
|
234 |
int matrix_value = rand()%(number_robots+1)-1; |
|
235 |
bom_matrix[ii][j] = matrix_value; |
|
236 |
} |
|
237 |
} |
|
238 |
*/ |
|
239 |
int number_robots = 3; |
|
240 |
int bom_matrix[number_robots][number_robots]; |
|
241 |
for (int ii = 0; ii < number_robots; ii++) { |
|
242 |
for (int j = 0; j < number_robots; j++) { |
|
243 |
bom_matrix[ii][j] = ((ii*3+j)%17)-1; |
|
244 |
} |
|
245 |
} |
|
227 |
int num_robots; |
|
228 |
int** bom_matrix = colonet_get_sensor_matrix(&num_robots); |
|
246 | 229 |
|
230 |
printf("number of robots is %d\n", num_robots); |
|
247 | 231 |
|
248 |
printf("number of robots is %d\n", number_robots); |
|
249 |
|
|
250 | 232 |
//TODO: make this better |
251 | 233 |
//TODO: make sure I don't need to do MAX_RESPONSE_LENGTH-1 |
252 |
snprintf(response_bom_matrix_buffer,MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_BOM_MATRIX, number_robots); |
|
253 |
for (int ii = 0; ii < number_robots; ii++) { |
|
254 |
for (int j = 0; j < number_robots; j++) { |
|
234 |
snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_BOM_MATRIX, |
|
235 |
num_robots); |
|
236 |
for (int i = 0; i < num_robots; i++) { |
|
237 |
for (int j = 0; j < num_robots; j++) { |
|
255 | 238 |
//TODO: don't use strcpy |
256 | 239 |
strcpy(temp_bom_matrix_buffer, response_bom_matrix_buffer); |
257 | 240 |
//TODO: put length checking in here so array doesn't go out of bounds |
258 | 241 |
//TODO: maybe use strncat? |
259 | 242 |
strcat(temp_bom_matrix_buffer," %d"); |
260 |
snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, temp_bom_matrix_buffer, bom_matrix[ii][j]);
|
|
243 |
snprintf(response_bom_matrix_buffer, MAX_RESPONSE_LEN, temp_bom_matrix_buffer, bom_matrix[i][j]); |
|
261 | 244 |
} |
262 | 245 |
} |
263 | 246 |
strcat(response_bom_matrix_buffer,"\n"); |
264 | 247 |
connection_pool->write_to_client(pool_index, response_bom_matrix_buffer, strlen(response_bom_matrix_buffer)); |
265 | 248 |
printf("Sending %s", response_bom_matrix_buffer); |
249 |
|
|
250 |
for (int i = 0; i < num_robots; i++) { |
|
251 |
free(bom_matrix[i]); |
|
252 |
} |
|
253 |
free(bom_matrix); |
|
266 | 254 |
|
267 | 255 |
return 0; |
268 | 256 |
} |
269 | 257 |
|
270 | 258 |
int Command::parse_request_xbee_ids(int pool_index) { |
271 |
//TODO: make this better |
|
272 |
//TODO: change when we actually know this data |
|
273 |
//int number_robots = rand() % 10; |
|
274 |
int number_robots = 3; |
|
275 |
|
|
259 |
char temp_xbee_id_buffer[MAX_RESPONSE_LEN]; |
|
276 | 260 |
char xbee_id_buffer[MAX_RESPONSE_LEN]; |
277 |
char temp_xbee_id_buffer[MAX_RESPONSE_LEN]; |
|
278 | 261 |
|
262 |
int num_robots; |
|
263 |
int* xbee_ids = colonet_get_xbee_ids(&num_robots); |
|
264 |
|
|
279 | 265 |
if (!connection_pool) { |
280 | 266 |
return -1; |
281 | 267 |
} |
282 | 268 |
|
283 |
snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, "%d %d %d", |
|
284 |
RESPONSE_TO_CLIENT_REQUEST, REQUEST_XBEE_IDS, number_robots); |
|
269 |
snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, "%d %d %d", RESPONSE_TO_CLIENT_REQUEST, REQUEST_XBEE_IDS, num_robots); |
|
285 | 270 |
|
286 |
printf("number of robots is %d\n", number_robots); |
|
287 |
|
|
288 |
for (int ii = 0; ii < number_robots; ii++) { |
|
271 |
for (int i = 0; i < num_robots; i++) { |
|
289 | 272 |
strcpy(temp_xbee_id_buffer, xbee_id_buffer); |
290 | 273 |
//TODO: put length checking in here so array doesn't go out of bounds |
291 | 274 |
//TODO: maybe use strncat? |
292 | 275 |
strcat(temp_xbee_id_buffer, " %d"); |
293 |
snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, temp_xbee_id_buffer, ii);
|
|
276 |
snprintf(xbee_id_buffer, MAX_RESPONSE_LEN, temp_xbee_id_buffer, xbee_ids[i]);
|
|
294 | 277 |
} |
295 | 278 |
strcat(xbee_id_buffer, "\n"); |
296 | 279 |
connection_pool->write_to_client(pool_index, xbee_id_buffer, strlen(xbee_id_buffer)); |
297 | 280 |
printf("Sending %s", xbee_id_buffer); |
298 | 281 |
|
282 |
free(xbee_ids); |
|
283 |
|
|
299 | 284 |
return 0; |
300 | 285 |
} |
trunk/code/projects/colonet/ColonetServer/wirelessMessageHandler.cpp | ||
---|---|---|
26 | 26 |
unsigned char* data, int len) { |
27 | 27 |
printf("Server received wireless message!!!\n"); |
28 | 28 |
|
29 |
return colonet_server.process_received_wireless_message(type, source, dest, |
|
30 |
data, len); |
|
29 |
printf("server received this packet: "); |
|
30 |
for (int i = 0; i < len; i++) { |
|
31 |
printf("%d ", data[i]); |
|
32 |
} |
|
33 |
printf(" type: %d, source: %d, dest: %d\n", type, source, dest); |
|
34 |
|
|
35 |
return colonet_server.process_received_wireless_message(type, source, dest, data, len); |
|
31 | 36 |
} |
trunk/code/projects/colonet/ColonetServer/ColonetServer.cpp | ||
---|---|---|
141 | 141 |
} |
142 | 142 |
|
143 | 143 |
int ColonetServer::process_received_wireless_message(unsigned char type, short source, int dest, |
144 |
unsigned char * data, int len) {
|
|
144 |
unsigned char * data, int len) {
|
|
145 | 145 |
if (type == COLONET_RESPONSE) { |
146 | 146 |
printf("response\n"); |
147 | 147 |
|
... | ... | |
151 | 151 |
snprintf(buffer, WRITE_BUFFER_SIZE, "%d\n", value); |
152 | 152 |
|
153 | 153 |
int buflen = strlen(buffer); |
154 |
if (connection_pool.write_to_client(dest, buffer, buflen) |
|
155 |
== ERROR_INVALID_CLIENT_ID) { |
|
156 |
printf("The robot wanted to pass the data to a client not in the " |
|
157 |
"pool.\n"); |
|
154 |
if (connection_pool.write_to_client(dest, buffer, buflen) == ERROR_INVALID_CLIENT_ID) { |
|
155 |
printf("The robot wanted to pass the data to a client not in the pool.\n"); |
|
158 | 156 |
return -1; |
159 | 157 |
} |
160 | 158 |
|
trunk/code/projects/libdragonfly/move.h | ||
---|---|---|
72 | 72 |
/** @} **/ |
73 | 73 |
|
74 | 74 |
#endif |
75 |
|
Also available in: Unified diff