reb.cc

机器人仿真软件

        newheading = true;
        this->desired_heading = heading_command;
      }
    
      if (this->velocity_mode) {
	// then we are in velocity mode
	
	if (!this->direct_velocity_control) {
	  // then we are doing my velocity based heading PD controller
	  
	  // calculate difference between desired and current
	  int diff = this->desired_heading - this->current_heading;
	  
	  // this will make diff the shortest angle between command and current
	  if (diff > 180) {
	    diff += -360; 
	  } else if (diff < -180) {
	    diff += 360; 
	  }
	  
	  int trans_long = (int) trans_command;
	  int rot_long = (int) rot_command;

	  // lets try to do this in fixed point
	  // max angle error is 180, so get a ratio
	  int err_ratio = diff*REB_FIXED_FACTOR / 180;
	  //double err_ratio = (double) diff / 180.0;

	  // choose trans speed inverse proportional to heading error
	  trans_long = (REB_FIXED_FACTOR - ABS(err_ratio))*trans_long;

	  // try doing squared error?
	  //	  trans_long = (REB_FIXED_FACTOR - err_ratio*err_ratio)*trans_long;

	  // now divide by factor to get regular value
	  trans_long /= REB_FIXED_FACTOR;
	  	  
	  // now we have to make a rotational velocity proportional to
	  // heading error with a damping term
	  
	  // there is a gain in here that maybe should be configurable
	  rot_long = err_ratio*3*rot_long;
	  rot_long /= REB_FIXED_FACTOR;
	  //rot_command = (short) (err_ratio*2.0*(double)rot_command);
	  
	  // make sure we stay within given limits

	  trans_command = (short)trans_long;
	  rot_command = (short) rot_long;

#ifdef DEBUG_POS
	  printf("REB: PD: diff=%d err=%d des=%d curr=%d trans=%d rot=%d\n", 
		 diff, err_ratio, this->desired_heading, this->current_heading, 
		 trans_command, rot_command);
#endif
	  
	  if (ABS(last_trans_command) - ABS(trans_command) < 0) {
	    // then we have to clip the new desired trans to given
	    // multiply by the sign just to take care of some crazy case
	    trans_command = SGN(trans_command)* last_trans_command;
	  }
	  
	  if (ABS(last_rot_command) - ABS(rot_command) < 0) {
	    rot_command = SGN(rot_command)*last_rot_command;
	  }
	}
	
	// so now we need to figure out left and right wheel velocities
	// to achieve the given trans and rot velocitties of the ubot
	long rot_term_fixed = rot_command * 
	  PlayerUBotRobotParams[this->param_index].RobotAxleLength / 2;

	rot_term_fixed = DEG2RAD_FIX(rot_term_fixed);

	leftvel = trans_command*REB_FIXED_FACTOR - rot_term_fixed;
	rightvel = trans_command*REB_FIXED_FACTOR + rot_term_fixed;
	

	leftvel /= REB_FIXED_FACTOR;
	rightvel /= REB_FIXED_FACTOR;

	int max_trans = PlayerUBotRobotParams[this->param_index].MaxVelocity;
	
	if (ABS(leftvel) > max_trans) {
	  if (leftvel > 0) {
	    leftvel = max_trans;
	    rightvel *= max_trans/leftvel;
	  } else {
	    leftvel = -max_trans;
	    rightvel *= -max_trans/leftvel;
	  }
	  
	  fprintf(stderr, "REB: left wheel velocity clipped\n");
	}
	
	if (ABS(rightvel) > max_trans) {
	  if (rightvel > 0) {
	    rightvel = max_trans;
	    leftvel *= max_trans/rightvel;
	  } else {
	    rightvel = -max_trans;
	    leftvel *= -max_trans/rightvel;
	  }

	  fprintf(stderr, "REB: right wheel velocity clipped\n");
	}
	
	// we have to convert from mm/s to pulse/10ms
	//	double left_velocity = (double) leftvel;
	//	double right_velocity = (double) rightvel;

	// add the RFF/2 for rounding
	long lvf = leftvel * PlayerUBotRobotParams[this->param_index].PulsesPerMMMSF + 
	  (REB_FIXED_FACTOR/2);
	long rvf = -(rightvel * PlayerUBotRobotParams[this->param_index].PulsesPerMMMSF +
		     (REB_FIXED_FACTOR/2));

	lvf /= REB_FIXED_FACTOR;
	rvf /= REB_FIXED_FACTOR;
	//	printf("REB: POS: lvel=%d rvel=%d ", leftvel, rightvel);
	//	left_velocity *= PlayerUBotRobotParams[this->param_index].PulsesPerMMMS;
	//	right_velocity *= PlayerUBotRobotParams[this->param_index].PulsesPerMMMS;
	
	//	right_velocity = -right_velocity;
	
	//	leftvel = (int) rint(left_velocity);
	//	rightvel = (int) rint(right_velocity);
	leftvel = lvf;
	rightvel = rvf;
	//	printf("REB: POS: %g [%d] lv=%d [%d] rv=%d [%d]\n", PlayerUBotRobotParams[this->param_index].PulsesPerMMMS,
	//	       PlayerUBotRobotParams[this->param_index].PulsesPerMMMSF,leftvel, lvf/REB_FIXED_FACTOR, rightvel, rvf/REB_FIXED_FACTOR);
#ifdef DEBUG_POS
	printf("REB: [%sABLED] VEL %s: lv=%d rv=%d trans=%d rot=%d\n", 
	       this->motors_enabled ? "EN" : "DIS",
	       this->direct_velocity_control ?
	       "DIRECT" : "PD", leftvel, rightvel, trans_command, rot_command);
#endif
	
	// now we set the speed
	if (this->motors_enabled) {
	  SetSpeed(REB_MOTOR_LEFT, leftvel);
	  SetSpeed(REB_MOTOR_RIGHT, rightvel);
	} else {
	  SetSpeed(REB_MOTOR_LEFT, 0);
	  SetSpeed(REB_MOTOR_RIGHT, 0);
	}
      } else {
	// we are in position mode....
	// we only do a translation or a rotation
	double lp, rp;

	// set this to false so that we catch the first on_target
	//	this->pos_mode_odom_update = false;

	newposcommand = false;
	// this will skip translation if command is 0
	// or if no new command
	if (newtrans) {
	  // then the command is a translation in mm
	  lp = (double) trans_command;
	  lp *= PlayerUBotRobotParams[this->param_index].PulsesPerMM;
	  leftpos = (int)rint(lp);

	  rp = (double) trans_command;
	  rp *= PlayerUBotRobotParams[this->param_index].PulsesPerMM;
	  rightpos = (int) rint(rp);

	  newposcommand = true;
	} else if (newrot) {
	  // then new rotation instead
	  // this rot command is in degrees
	  lp = -DEG2RAD((double)rot_command)*
	    PlayerUBotRobotParams[this->param_index].RobotAxleLength/2.0 *
	    PlayerUBotRobotParams[this->param_index].PulsesPerMM;
	  rp = -lp;
	  
	  leftpos = (int) rint(lp);
	  rightpos = (int) rint(rp);

	  newposcommand = true;
	}

#ifdef DEBUG_POS	
	printf("REB: [%sABLED] POSITION leftpos=%d rightpos=%d\n",
	       this->motors_enabled ? "EN" : "DIS", leftpos, rightpos);
#endif

	// now leftpos and rightpos are the right positions to reach
	// reset the counters first???? FIX  
	// we have to return the position command status now FIX
	if (this->motors_enabled && newposcommand) {
	  printf("REB: SENDING POS COMMAND l=%d r=%d\n", leftpos, rightpos);
	  // we need to reset counters to 0 for odometry to work
	  SetPosCounter(REB_MOTOR_LEFT, 0);
	  SetPosCounter(REB_MOTOR_RIGHT, 0);
	  SetPos(REB_MOTOR_LEFT, leftpos);
	  SetPos(REB_MOTOR_RIGHT, -rightpos);
	}
      }
    }
  return 0;    
}

// Process incoming messages from clients 
int REB::ProcessMessage(ClientData * client, player_msghdr * hdr, uint8_t * data, uint8_t * resp_data, size_t * resp_len)
{
  assert(hdr);
  assert(data);
  assert(resp_data);
  assert(resp_len);
  
  if (MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 0, position_id))
  {
  	assert(hdr->size == sizeof(player_position_cmd_t));
	ProcessCommand(reinterpret_cast<player_position_cmd_t *> (data));
	*resp_len = 0;
	return 0;
  }

  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_POWER, ir_id))
  {
  	assert(hdr->size == sizeof(player_ir_power_req_t));
  	player_ir_power_req_t * powreq = reinterpret_cast<player_ir_power_req_t *> (data);
  	
    if (powreq->state) 
      SetIRState(REB_IR_START);
    else 
      SetIRState(REB_IR_STOP);
      
    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }
  
  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_POSE, ir_id))
  {
  	assert(*resp_len >= sizeof(player_ir_pose_t));
  	player_ir_pose_t & irpose = *reinterpret_cast<player_ir_pose_t *> (resp_data);

    uint16_t numir = PlayerUBotRobotParams[param_index].NumberIRSensors;
    irpose.pose_count = htons(numir);
    for (int i =0; i < numir; i++) {
      int16_t *irp = PlayerUBotRobotParams[param_index].ir_pose[i];
      for (int j =0; j < 3; j++) {
        irpose.poses[i][j] = htons(irp[j]);
      }
    }

    *resp_len = sizeof(player_ir_pose_t);
    return PLAYER_MSGTYPE_RESP_ACK;
  }
  
  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_GET_GEOM, position_id))
  {
/*  	assert(hdr->size == sizeof(player_position_geom_t));
  	player_position_geom_t * req = reinterpret_cast<player_position_geom_t *> (data);*/
  	assert(*resp_len >= sizeof(player_position_geom_t));
  	player_position_geom_t & geom = *reinterpret_cast<player_position_geom_t *> (resp_data);

    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));

    *resp_len = sizeof(player_position_geom_t);
    return PLAYER_MSGTYPE_RESP_ACK;
  }

  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_MOTOR_POWER, position_id))
  {
  	assert(hdr->size == sizeof(player_position_power_config_t));
  	player_position_power_config_t * mpowreq = reinterpret_cast<player_position_power_config_t *> (data);

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

    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }

          // select method of velocity control
          // 0 for direct velocity control (trans and rot applied directly)
          // 1 for builtin velocity based heading PD controller
  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_VELOCITY_MODE, position_id))
  {
    assert(hdr->size == sizeof(player_position_velocitymode_config_t));
  	player_position_velocitymode_config_t * velcont = reinterpret_cast<player_position_velocitymode_config_t *> (data);

    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;
    
    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }

  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_RESET_ODOM, position_id))
  {
    SetOdometry(0,0,0);

    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }
  
  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_POSITION_MODE, position_id))
  {
  	assert(hdr->size == sizeof(player_position_position_mode_req_t));
  	player_position_position_mode_req_t * posmode = reinterpret_cast<player_position_position_mode_req_t *> (data);

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

    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }
          
  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_SET_ODOM, position_id))
  {
  	assert(hdr->size == sizeof(player_position_set_odom_req_t));
  	player_position_set_odom_req_t * req = reinterpret_cast<player_position_set_odom_req_t *> (data);

#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);

    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }

  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_SPEED_PID, position_id))
  {
  	assert(hdr->size == sizeof(player_position_speed_pid_req_t));
  	player_position_speed_pid_req_t * pid = reinterpret_cast<player_position_speed_pid_req_t *> (data);

    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);

    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }

  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_POSITION_PID, position_id))
  {
  	assert(hdr->size == sizeof(player_position_position_pid_req_t));
  	player_position_position_pid_req_t * pid = reinterpret_cast<player_position_position_pid_req_t *> (data);

    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);

    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }

  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_SPEED_PROF, position_id))
  {
  	assert(hdr->size == sizeof(player_position_speed_prof_req_t));
  	player_position_speed_prof_req_t * prof = reinterpret_cast<player_position_speed_prof_req_t *> (data);

    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

    *resp_len = 0;
    return PLAYER_MSGTYPE_RESP_ACK;
  }

  *resp_len = 0;
  return -1;
}


/* this will read a new config command and interpret it
 *
 * returns: 
 */
/*void
REB::ReadConfig()
{
  int config_size;
  unsigned char config_buffer[REB_CONFIG_BUFFER_SIZE];
  void *client;

  // check for IR config requests
  if((config_size = GetConfig(this->ir_id, &client, 
                              (void*)config_buffer, 
                              sizeof(config_buffer),NULL))) 
  {
    // REB_IR IOCTLS /////////////////
      
#ifdef DEBUG_CONFIG
    printf("REB: IR CONFIG\n");
#endif

    // figure out which command
    switch(config_buffer[0]) 
    {
      case PLAYER_IR_POWER_REQ: 
        {
          // request to change IR state
          // 1 means turn on
          // 0 is off
          if (config_size != sizeof(player_ir_power_req_t)) 
          {
            fprintf(stderr, "REB: argument to IR power req wrong size (%d)\n", config_size);
            if(PutReply(this->ir_id, client, PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to reply");
            break;
          }

          player_ir_power_req_t *powreq = (player_ir_power_req_t *)config_buffer;	
#ifdef DEBUG_CONFIG
          printf("REB: IR_POWER_REQ: %d\n", powreq->state);
#endif

          if (powreq->state) {
            SetIRState(REB_IR_START);
          } else {
            SetIRState(REB_IR_STOP);
          }

          if(PutReply(this->ir_id, client, PLAYER_MSGTYPE_RESP_ACK, NULL))
            PLAYER_ERROR("REB: failed to reply");
        }
        break;
      
      case PLAYER_IR_POSE_REQ: 
        {
          // request the pose of the IR sensors in robot-centric coords
          if(config_size != sizeof(player_ir_pose_req_t)) 
          {
            fprintf(stderr, "REB: argument to IR pose req wrong size (%d)\n", config_size);
            if(PutReply(this->ir_id, client, PLAYER_MSGTYPE_RESP_NACK, NULL))
              PLAYER_ERROR("REB: failed to put reply");
            break;
          }

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

          player_ir_pose_t irpose;
          uint16_t numir = PlayerUBotRobotParams[param_index].NumberIRSensors;
          irpose.pose_count = htons(numir);
          for (int i =0; i < numir; i++) {
            int16_t *irp = PlayerUBotRobotParams[param_index].ir_pose[i];
            for (int j =0; j < 3; j++) {
              irpose.poses[i][j] = htons(irp[j]);
            }
          }

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

      default:
        fprintf(stderr, "REB: IR got unknown config\n");
        if(PutReply(this->ir_id, client, PLAYER_MSGTYPE_RESP_NACK, NULL))
          PLAYER_ERROR("REB: failed to put reply");
        break;
    }
    // END REB_IR IOCTLS //////////////
  }

  // check for position config requests
  if((config_size = GetConfig(this->position_id, &client, 
                              (void*)config_buffer, 
                              sizeof(config_buffer),NULL))) 

  {
      // POSITION IOCTLS ////////////////
#ifdef DEBUG_CONFIG