reb.cc

机器人仿真平台,和stage配合运行

	SetSpeed(REB_MOTOR_RIGHT, 0);
	
	SetOdometry(0,0,0);
      } else if (this->pos_subscriptions && !(pos->subscriptions)) {
	// last sub just unsubbed
	printf("REB: last pos sub gone\n");
	SetSpeed(REB_MOTOR_LEFT, 0);
	SetSpeed(REB_MOTOR_RIGHT, 0);
	
        // overwrite existing motor commands to be zero
        player_position_cmd_t position_cmd;
        position_cmd.xspeed = 0;
        position_cmd.yawspeed = 0;
	position_cmd.yaw = 0;
        // TODO: who should really be the client here?
        pos->PutCommand(this,(unsigned char*)(&position_cmd), 
			sizeof(position_cmd));
      }
      
      this->pos_subscriptions = pos->subscriptions;
    } 

    if (power) {
      if (!this->power_subscriptions && power->subscriptions) {
	printf("REB: POWER SUBSCRIPTION\n");
	this->power_subscriptions = power->subscriptions;
      }
    }

    // get configuration commands (ioctls)
    ReadConfig();

    /* read the clients' commands from the common buffer */
    GetCommand((unsigned char*)&cmd, sizeof(cmd));

    bool newtrans = false, newrot = false, newheading=false;
    bool newposcommand=false;
    short trans_command, rot_command, heading_command;
    
    if ((trans_command = (short)ntohs(cmd.position.xspeed)) != 
	last_trans_command) {
      newtrans = true;
      last_trans_command = trans_command;
    }
    
    if ((rot_command = (short) ntohs(cmd.position.yawspeed)) != 
	last_rot_command) {
      newrot = true;
      last_rot_command = rot_command;
    }
    
    if ((heading_command = (short) ntohs(cmd.position.yaw)) != 
	this->desired_heading) {
      newheading = true;
      this->desired_heading = heading_command;
    }
    
    if (this->pos_subscriptions) {
      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
	  unsigned short current_theta = (short) ntohs(this->data->position.yaw);
	  int diff = this->desired_heading - current_theta;
	  
	  // 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, current_theta, 
		 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\n");
	  // 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);
	}
      }
    }
    
    pthread_testcancel();

    // now lets get new data...
    UpdateData();

    pthread_testcancel();
    
  }
  pthread_exit(NULL);
}

/* start a thread that will invoke Main() */
void 
REB::StartThread()
{
  pthread_create(&thread, NULL, &DummyMain, this);
}

/* cancel (and wait for termination) of the thread */
void 
REB::StopThread()
{
  void* dummy;
  pthread_cancel(thread);
  if(pthread_join(thread,&dummy))
    perror("REB::StopThread:pthread_join()");
}


/* 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];
  player_device_id_t id;
  void *client;

  if ((config_size = GetConfig(&id, &client, 
			       (void *)config_buffer, sizeof(config_buffer)))) {
    
    // figure out which device it's for
    switch(id.code) {
      
      // REB_IR IOCTLS /////////////////
    case PLAYER_IR_CODE:
      
#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(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    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(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  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(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    PLAYER_ERROR("REB: failed to put reply");
	    break;
	  }
	}
	
#ifdef DEBUG_CONFIG
	printf("REB: IR_POSE_REQ\n");
#endif

	player_ir_pose_t irpose;
	for (int i =0; i < PLAYER_IR_MAX_SAMPLES; i++) {
	  ir_pose_t irp = PlayerUBotRobotParams[param_index].ir_pose[i];
	  
	  irpose.poses[i][0] = htons((short) irp.ir_x);
	  irpose.poses[i][1] = htons((short) irp.ir_y);
	  irpose.poses[i][2] = htons((short) irp.ir_theta);
	}
	
	if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, &irpose,
		     sizeof(irpose))) {
	  PLAYER_ERROR("REB: failed to put reply");
	  break;
	}
      }
      break;

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

      // POSITION IOCTLS ////////////////
    case PLAYER_POSITION_CODE:
#ifdef DEBUG_CONFIG
      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);
	  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(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom,
		     sizeof(geom))) {
	  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(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    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(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  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(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0)) {
	    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;
	this->pos_update_period = REB_POS_UPDATE_PERIOD_VEL;
	
	if (PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0)) {
	  PLAYER_ERROR("REB: failed to put reply");
	}
      }
      break;

      case PLAYER_POSITION_RESET_ODOM_REQ: {