Project

General

Profile

Revision 518

fixed mutex unlock bug. made it so that if there is just one robot seen, it is assumed to be the robot requesting

View differences:

colonet_wireless.cpp
106 106
  }
107 107

  
108 108
  if (dest == GLOBAL_DEST) {
109
    printf("sending to global dest\n");
109
    //printf("sending to global dest\n");
110 110
    if (wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
111 111
                              sizeof(ColonetRobotServerPacket), 0) != 0) {
112 112
      return -1;
113 113
    }
114 114
  } else {
115
    printf("sending to specific robot: %d.\n", dest);
115
    //("sending to specific robot: %d.\n", dest);
116 116
    if (wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
117 117
      sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID) != 0) {
118 118
      return -1;
......
180 180
  //TODO: These are just here to get rid of compiler warnings
181 181
  type = type;
182 182

  
183
  printf("!!!***Got packet from robots***!!!\n");
183
  //printf("!!!***Got packet from robot***!!!\n");
184 184

  
185 185
  ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data;
186 186

  
......
191 191
  if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {
192 192
    /* Robot has requested its position. */
193 193
    int robot_x, robot_y;
194
    if (colonet_server.getPositionMonitor()->getRobotPosition(source, &robot_x, &robot_y) != 0) {
194
    PositionMonitor* pm = colonet_server.getPositionMonitor();
195
    int num_robots = pm->getNumVisibleRobots();
196
    int ret = pm->getRobotPosition(source, &robot_x, &robot_y);
197
    if (ret != 0 && num_robots != 1) {
195 198
      fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source);
196 199
    } else {
200
      if (ret != 0 /* && num_robots == 1 */) {
201
        VisionPosition pos = pm->getFirstPosition();
202
        robot_x = pos.x;
203
        robot_y = pos.y;
204
      }
205

  
197 206
      unsigned char response[80];
198
      response[0] = robot_x & 0xFF;
199
      response[1] = (robot_x >> 8) & 0xFF;
200
      response[2] = robot_y & 0xFF;
201
      response[3] = (robot_y >> 8) & 0xFF;
207
      response[0] = (robot_x >> 8) & 0xFF;
208
      response[1] = robot_x & 0xFF;
209
      response[2] = (robot_y >> 8) & 0xFF;
210
      response[3] = robot_y & 0xFF;
202 211

  
212
      fprintf(stderr, "Robot %d requested position.  Its position is %d,%d.\n", source, robot_x, robot_y);
213

  
203 214
      if (colonet_wl_send(-1, source, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) {
204 215
        fprintf(stderr, "colonet_wl_send failed!\n");
205 216
        exit(1);

Also available in: Unified diff