reb.cc

机器人仿真软件

    printf("REB: POSITION CONFIG\n");
#endif
    switch (config_buffer[0]) 
    {
      case PLAYER_POSITION_GET_GEOM_REQ: 
        {
          // get geometry of robot
          if (config_size != sizeof(player_position_geom_t)) 
          {
            fprintf(stderr, "REB: get geom req is wrong size (%d)\n", config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

#ifdef DEBUG_CONFIG
          printf("REB: POSITION_GET_GEOM_REQ\n");
#endif

          player_position_geom_t geom;
          geom.subtype = PLAYER_POSITION_GET_GEOM_REQ;
          geom.pose[0] = htons(0);
          geom.pose[1] = htons(0);
          geom.pose[2] = htons(0);
          geom.size[0] = geom.size[1] = 
                  htons( (short) (2 * PlayerUBotRobotParams[this->param_index].RobotRadius));

          if(PutReply(this->position_id, client, PLAYER_MSGTYPE_RESP_ACK, 
                      &geom, sizeof(geom), NULL)) 
            PLAYER_ERROR("REB: failed to put reply");
        }
        break;

      case PLAYER_POSITION_MOTOR_POWER_REQ: 
        {
          // change motor state
          // 1 for on 
          // 0 for off

          if (config_size != sizeof(player_position_power_config_t)) 
          {
            fprintf(stderr, "REB: pos motor power req got wrong size (%d)\n", config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

          player_position_power_config_t *mpowreq = (player_position_power_config_t *)config_buffer;

#ifdef DEBUG_CONFIG
          printf("REB: MOTOR_POWER_REQ %d\n", mpowreq->value);
#endif

          if (mpowreq->value) {
            this->motors_enabled = true;
          } else {
            this->motors_enabled = false;
          }

          if(PutReply(this->position_id, client, PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to put reply");

          printf("REB: put MOTOR POWER REQ\n");
        }
        break;

      case PLAYER_POSITION_VELOCITY_MODE_REQ: 
        {
          // select method of velocity control
          // 0 for direct velocity control (trans and rot applied directly)
          // 1 for builtin velocity based heading PD controller
          if (config_size != sizeof(player_position_velocitymode_config_t)) 
          {
            fprintf(stderr, "REB: pos vel control req got wrong size (%d)\n", config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

          player_position_velocitymode_config_t *velcont = 
                  (player_position_velocitymode_config_t *) config_buffer;

#ifdef DEBUG_CONFIG
          printf("REB: VELOCITY_MODE_REQ %d\n", velcont->value);
#endif

          if (!velcont->value) {
            this->direct_velocity_control = true;
          } else {
            this->direct_velocity_control = false;
          }

          // also set up not to use position mode!
          this->velocity_mode = true;

          if(PutReply(this->position_id, client, 
                      PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to put reply");
        }
        break;

      case PLAYER_POSITION_RESET_ODOM_REQ: 
        {
          // reset the odometry
          if (config_size != sizeof(player_position_resetodom_config_t)) 
          {
            fprintf(stderr, "REB: pos reset odom req got wrong size (%d)\n", config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

#ifdef DEBUG_CONFIG
          printf("REB: RESET_ODOM_REQ\n");
#endif

          SetOdometry(0,0,0);

          if(PutReply(this->position_id, client, 
                      PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to put reply");
        }
        break;
      
      case PLAYER_POSITION_POSITION_MODE_REQ: 
        {
          // select velocity or position mode
          // 0 for velocity mode
          // 1 for position mode
          if (config_size != sizeof(player_position_position_mode_req_t)) 
          {
            fprintf(stderr, "REB: pos vel mode req got wrong size (%d)\n", config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

          player_position_position_mode_req_t *posmode = 
                  (player_position_position_mode_req_t *)config_buffer;
#ifdef DEBUG_CONFIG
          printf("REB: POSITION_MODE_REQ %d\n", posmode->state);
#endif

          if (posmode->state) {
            this->velocity_mode = false;
          } else {
            this->velocity_mode = true;
          }

          if(PutReply(this->position_id, client, PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to put reply");
        }
        break;

      case PLAYER_POSITION_SET_ODOM_REQ: 
        {
          // set the odometry to a given position
          if (config_size != sizeof(player_position_set_odom_req_t)) 
          {
            fprintf(stderr, "REB: pos set odom req got wrong size (%d)\n",
                    config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

          player_position_set_odom_req_t *req = 
                  (player_position_set_odom_req_t *)config_buffer;

#ifdef DEBUG_CONFIG
          int x,y;
          short theta;
          x = ntohl(req->x);
          y = ntohl(req->y);
          theta = ntohs(req->theta);

          printf("REB: SET_ODOM_REQ x=%d y=%d theta=%d\n", x, y, theta);
#endif
          SetOdometry(req->x, req->y, req->theta);

          if(PutReply(this->position_id, client, 
                      PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to put reply");
        }
        break;

      case PLAYER_POSITION_SPEED_PID_REQ: 
        {
          // set up the velocity PID on the REB
          // kp, ki, kd are used
          if (config_size != sizeof(player_position_speed_pid_req_t)) 
          {
            fprintf(stderr, "REB: pos speed PID req got wrong size (%d)\n", config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

          player_position_speed_pid_req_t *pid = 
                  (player_position_speed_pid_req_t *)config_buffer;
          int kp = ntohl(pid->kp);
          int ki = ntohl(pid->ki);
          int kd = ntohl(pid->kd);

#ifdef DEBUG_CONFIG
          printf("REB: SPEED_PID_REQ kp=%d ki=%d kd=%d\n", kp, ki, kd);
#endif

          ConfigSpeedPID(REB_MOTOR_LEFT, kp, ki, kd);
          ConfigSpeedPID(REB_MOTOR_RIGHT, kp, ki, kd);

          if(PutReply(this->position_id, client, 
                      PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to put reply");
        }
        break;
      
      case PLAYER_POSITION_POSITION_PID_REQ: 
        {
          // set up the position PID on the REB
          // kp, ki, kd are used
          if(config_size != sizeof(player_position_position_pid_req_t)) 
          {
            fprintf(stderr, "REB: pos pos PID req got wrong size (%d)\n", config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

          player_position_position_pid_req_t *pid = 
                  (player_position_position_pid_req_t *)config_buffer;
          int kp = ntohl(pid->kp);
          int ki = ntohl(pid->ki);
          int kd = ntohl(pid->kd);

#ifdef DEBUG_CONFIG
          printf("REB: POS_PID_REQ kp=%d ki=%d kd=%d\n", kp, ki, kd);
#endif

          ConfigPosPID(REB_MOTOR_LEFT, kp, ki, kd);
          ConfigPosPID(REB_MOTOR_RIGHT, kp, ki, kd);

          if(PutReply(this->position_id, client, 
                      PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to put reply");
        }
        break;

      case PLAYER_POSITION_SPEED_PROF_REQ: 
        {
          // set the speed profile for position mode 
          // speed is max speed
          // acc is max acceleration
          if (config_size != sizeof(player_position_speed_prof_req_t)) 
          {
            fprintf(stderr, "REB: pos speed prof req got wrong size (%d)\n", config_size);
            if(PutReply(this->position_id, client, 
                        PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

          player_position_speed_prof_req_t *prof = 
                  (player_position_speed_prof_req_t *)config_buffer;	
          int spd = ntohs(prof->speed);
          int acc = ntohs(prof->acc);

#ifdef DEBUG_CONFIG	
          printf("REB: SPEED_PROF_REQ: spd=%d acc=%d  spdu=%g accu=%g\n", spd, acc,
                 spd*PlayerUBotRobotParams[this->param_index].PulsesPerMMMS,
                 acc*PlayerUBotRobotParams[this->param_index].PulsesPerMMMS);
#endif
          // have to convert spd from mm/s to pulse/10ms
          spd = (int) rint((double)spd * 
                           PlayerUBotRobotParams[this->param_index].PulsesPerMMMS);

          // have to convert acc from mm/s^2 to pulses/256/(10ms^2)
          acc = (int) rint((double)acc * 
                           PlayerUBotRobotParams[this->param_index].PulsesPerMMMS);
          // now have to turn into pulse/256
          //	acc *= 256;

          if (acc > REB_MAX_ACC) {
            acc = REB_MAX_ACC;
          } else if (acc == 0) {
            acc = REB_MIN_ACC;
          }

#ifdef DEBUG_CONFIG
          printf("REB: SPEED_PROF_REQ: SPD=%d  ACC=%d\n", spd, acc);
          ConfigSpeedProfile(REB_MOTOR_LEFT, spd, acc);
          ConfigSpeedProfile(REB_MOTOR_RIGHT, spd, acc);
#endif
          if(PutReply(this->position_id, client, PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to put reply");
        }
        break;
      
      default:
	fprintf(stderr, "REB: got unknown position config command\n");
        if(PutReply(this->position_id, client, PLAYER_MSGTYPE_RESP_NACK, NULL))
          PLAYER_ERROR("REB: failed to put reply");
        break;
    }
    // END REB_POSITION IOCTLS ////////////
  }
}
*/

/* this will update the data that is sent to clients
 * just call separate functions to take care of it
 *
 * returns:
 */
void
REB::UpdateData()
{
  player_position_data_t position_data;
  player_ir_data_t ir_data;
  player_power_data_t power_data;

  UpdateIRData(&ir_data);
  PutMsg(this->ir_id, NULL, PLAYER_MSGTYPE_DATA, 0,
          (void*)&ir_data,
          sizeof(player_ir_data_t),
          NULL);

  UpdatePowerData(&power_data);
  PutMsg(this->power_id, NULL, PLAYER_MSGTYPE_DATA, 0,
          (void*)&power_data,
          sizeof(player_power_data_t),
          NULL);

  UpdatePosData(&position_data);
  PutMsg(this->position_id, NULL, PLAYER_MSGTYPE_DATA, 0,
          (void*)&position_data,
          sizeof(player_position_data_t),
          NULL);
}

/* this will update the IR part of the client data
 * it entails reading the currently active IR sensors
 * and then changing their state to off and turning on
 * 2 new IRs.  
 * NOTE: assumes calling function already called Lock()
 *
 * returns:
 */
void
REB::UpdateIRData(player_ir_data_t * d)
{
  // then we can take a reading
  uint16_t volts[PLAYER_IR_MAX_SAMPLES];

  ReadAllIR(volts);
  
  for (int i =0; i < PLAYER_IR_MAX_SAMPLES; i++) {
    // these are in units of 4 mV
    // now turn into mV units
    volts[i] *= 4;
    d->voltages[i] = htons(volts[i]);
  }
  
}

/* this will update the POWER data.. basically just the battery level for now
 * NOTE: assumes calling function already called Lock()
 * returns:
 */
void
REB::UpdatePowerData(player_power_data_t *d)
{
  // read voltage 
  uint16_t volt = (uint16_t)ReadAD(REB_BATTERY_CHANNEL);
  
  // this is in units of 20mV.. change to mV
  volt *= 20;
  d->charge = htons(volt);
}
  
/* this will update the position data.  this entails odometry, etc
 * assumes caller already called Lock()
 * It is in the midst of being converted from floating to fixed point...
 * returns:
 */ 
void
REB::UpdatePosData(player_position_data_t *d)
{
  // change to fixed point FIX
  double x=0, y=0, theta;
  long x_f, y_f;
  unsigned char target_status =0;
  int lreading=0, rreading=0;
  long mmpp_f = PlayerUBotRobotParams[this->param_index].MMPerPulsesF;
  //  static int last_lpos=0, last_rpos=0;
  int x_rem=0, y_rem=0;

  // check if we have to get a baseline time first
  if (this->refresh_last_position) {
    GlobalTime->GetTime(&(this->last_position));
    this->refresh_last_position = false;
  }
  
  // get the previous odometry values
  // we know this is from last time, cuz this function
  // is only place to change them
  //  theta = (double) ((int)ntohs(this->data->position.yaw));

  //covert theta to rad
  //  theta = DEG2RAD(theta);
  theta = last_theta;

  //  x_f = ntohl(this->data->position.xpos)*REB_FIXED_FACTOR ;
  //  y_f = ntohl(this->data->position.ypos)*REB_FIXED_FACTOR ;
  x_f = last_x_f;
  y_f = last_y_f;

  // get the time
  struct timeval curr;
  GlobalTime->GetTime(&curr);

  double v, theta_dot;
  long v_f=0;

  if (this->velocity_mode) {
    int lpos=0, rpos=0, lp, rp;
    //        lvel = ReadSpeed(REB_MOTOR_LEFT);
    lpos = ReadPos(REB_MOTOR_LEFT);
    // negate because motor's are facing opposite directions
    //        rvel = -ReadSpeed(REB_MOTOR_RIGHT); 
    rpos = -ReadPos(REB_MOTOR_RIGHT);

    lreading = lpos;
    rreading = rpos;

    // calc time in  sec
    long t_f = (curr.tv_sec - this->last_position.tv_sec)*100 +
      (curr.tv_usec - this->last_position.tv_usec)/10000;

    lp = lpos-last_lpos;
    rp = rpos-last_rpos;
    
    last_lpos = lpos;
    last_rpos = rpos;

    // this is pulse/10ms
    v_f = (rp+lp) * REB_FIXED_FACTOR / 2;
    v_f /= t_f;

    //    v_f = (rvel+lvel)/2;
    //    v_f *= REB_FIXED_FACTOR;

    // rad/pulse
    //    theta_dot = (rvel- lvel) /
    //    theta_dot = (( (double) rp / (double) t_f) - ( (double)lp / (double) t_f)) /
    //      (PlayerUBotRobotParams[this->param_index].RobotAxleLength *
    //       PlayerUBotRobotParams[this->param_index].PulsesPerMM);
    theta_dot = (rp - lp) / 
      (PlayerUBotRobotParams[param_index].RobotAxleLength *
       PlayerUBotRobotParams[param_index].PulsesPerMM * (double)t_f);

    theta += theta_dot * t_f;

    // convert from rad/10ms -> rad/s -> deg/s
    theta_dot *= 100.0;
    
    // this is pulse/10ms
    long x_dot_f = (long) (v_f * cos(theta));
    long y_dot_f = (long) (v_f * sin(theta));

    // change to deltas mm and add integrate over time
      
    //    x_f += (x_dot_f/REB_FIXED_FACTOR) * mmpp_f * t_f;
    //    y_f += (y_dot_f/REB_FIXED_FACTOR) * mmpp_f * t_f;

    int base = (mmpp_f * t_f);
    x_rem = base * (x_dot_f /100);
    assert(ABS(x_rem) <= 0x7FFFFFFF);
    x_rem /= 100;

    y_rem = base * (y_dot_f / 100);
    assert(ABS(y_rem) <= 0x7FFFFFFF);
    y_rem /= 100;

    x_f += x_rem;
    y_f += y_rem;

    last_x_f = x_f;
    last_y_f = y_f;
    last_theta = theta;

    //    printf("REB: x=%d y=%d t=%g lp=%d rp=%d t_f=%d vf=%d xd=%d yd=%d xr=%d yr=%d\n", x_f, y_f, last_theta, lp, rp, t_f, v_f,
    //	   x_dot_f, y_dot_f, x_rem, y_rem);

    x_f /= REB_FIXED_FACTOR;
    y_f /= REB_FIXED_FACTOR;
  } else {
    // in position mode
    int rpos=0, lpos=0;
    uint8_t rtar;
    int mode, error;
    
    v = theta_dot= 0.0;

    // now we read the status of the motion controller
    // DONT ASK ME -- but calling ReadStatus on the LEFT
    // motor seems to cause the REB (the kameleon itself!)
    // to freeze some time after issuing a position mode
    // command -- happens for RIGHT motor too but
    // maybe not as much???
    //  ltar = ReadStatus(REB_MOTOR_LEFT, &mode, &error);
    
    rtar = ReadStatus(REB_MOTOR_RIGHT, &mode, &error);

    target_status = rtar;
    // check for on target so we know to update
    //    if (!d->position.stall && target_status) {
      // then this is a new one, so do an update

      lpos = ReadPos(REB_MOTOR_LEFT);
      rpos = -ReadPos(REB_MOTOR_RIGHT);

      lreading = lpos;
      rreading = rpos;
      double p;