Project

General

Profile

Revision 297

trying to get server to send back num robots, xbee ids, sensor matrix -- not working yet

View differences:

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