wbr914.cc

机器人仿真软件

		  sizeof(player_position2d_data_t),
		  NULL);
  }

  if ( ir_subscriptions )
  {
    // put ir data
    this->Publish(this->ir_id, NULL,
		  PLAYER_MSGTYPE_DATA,
		  PLAYER_IR_DATA_RANGES,
		  (void*)&(_data.ir),
		  sizeof(_data.ir),
		  NULL);
  }

  if ( aio_subscriptions )
  {
    // put Analog Input data
    this->Publish(this->aio_id, NULL,
		  PLAYER_MSGTYPE_DATA,
		  PLAYER_AIO_DATA_STATE,
		  (void*)&(_data.aio),
		  sizeof(_data.aio),
		  NULL);
  }

  if ( dio_subscriptions )
  {
    // put Digital Input data
    this->Publish(this->dio_id, NULL,
		  PLAYER_MSGTYPE_DATA,
		  PLAYER_DIO_DATA_VALUES,
		  (void*)&(_data.dio),
		  sizeof(_data.dio),
		  NULL);
  }
}

void wbr914::Main()
{
  int last_position_subscrcount=0;

  if ( _debug )
    PLAYER_MSG0( 0, "Main\n" );

  for(;;)
  {
    pthread_testcancel();

    // we want to reset the odometry and enable the motors if the first
    // client just subscribed to the position device, and we want to stop
    // and disable the motors if the last client unsubscribed.
    if(!last_position_subscrcount && this->position_subscriptions)
    {
      this->EnableMotors( false );
      this->ResetRawPositions();
    }
    else if(last_position_subscrcount && !(this->position_subscriptions))
    {
      // enable motor power
      if ( _debug )
	PLAYER_MSG0( 0, "enabling motors\n" );
      this->EnableMotors( true );
    }
    last_position_subscrcount = this->position_subscriptions;

    this->Unlock();

    // handle pending messages
    if(!this->InQueue->Empty())
    {
      if ( _debug )
	PLAYER_MSG0( 0, "processing messages\n" );
      ProcessMessages();
    }
    else
    {
      // if no pending msg, resend the last position cmd.
      // TODO: check if this is initialized
      //      this->HandlePositionCommand( this->last_position_cmd );
    }

    if ( _debug )
      PLAYER_MSG0( 0, "GetAllData\n" );

    GetAllData();

    if ( _debug )
      PLAYER_MSG0( 0, "PublishData\n" );

    PublishData();
  }
}

int wbr914::ProcessMessage(MessageQueue * resp_queue,
			   player_msghdr * hdr,
			   void * data)
{
  // Look for configuration requests
  if(hdr->type == PLAYER_MSGTYPE_REQ)
    return(this->HandleConfig(resp_queue,hdr,data));
  else if(hdr->type == PLAYER_MSGTYPE_CMD)
    return(this->HandleCommand(hdr,data));


  return(-1);
}

int wbr914::HandleConfig(MessageQueue* resp_queue,
			 player_msghdr * hdr,
			 void * data)
{
  if ( _debug )
    printf( "HandleConfig\n" );

  // check for position config requests
  if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                           PLAYER_POSITION2D_REQ_SET_ODOM,
                           this->position_id))
  {
    if(hdr->size != sizeof(player_position2d_set_odom_req_t))
    {
      PLAYER_WARN("Arg to odometry set requests wrong size; ignoring");
      return(-1);
    }

    SetOdometry( (player_position2d_set_odom_req_t*)data );
    Publish( position_id, resp_queue,
	     PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM);
    UpdateM3();
    return(0);
  }
  else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                                PLAYER_POSITION2D_REQ_MOTOR_POWER,
                                this->position_id))
  {
    /* motor state change request
     *   1 = enable motors
     *   0 = disable motors (default)
     */
    if(hdr->size != sizeof(player_position2d_power_config_t))
    {
      PLAYER_WARN("Arg to motor state change request wrong size; ignoring");
      return(-1);
    }
    player_position2d_power_config_t* power_config =
            (player_position2d_power_config_t*)data;
    this->EnableMotors( power_config->state == 1 );

    this->Publish(this->position_id, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
    UpdateM3();
    return(0);
  }
  else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                                PLAYER_POSITION2D_REQ_RESET_ODOM,
                                this->position_id))
  {
    /* reset position to 0,0,0: no args */
    if(hdr->size != 0)
    {
      PLAYER_WARN("Arg to reset position request is wrong size; ignoring");
      return(-1);
    }
    ResetRawPositions();

    this->Publish(this->position_id, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM);
    UpdateM3();
    return(0);
  }
  else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                                PLAYER_POSITION2D_REQ_GET_GEOM,
                                this->position_id))
  {
    /* Return the robot geometry. */
    if(hdr->size != 0)
    {
      PLAYER_WARN("Arg get robot geom is wrong size; ignoring");
      return(-1);
    }

    Publish(this->position_id, resp_queue,
	    PLAYER_MSGTYPE_RESP_ACK,
	    PLAYER_POSITION2D_REQ_GET_GEOM,
	    (void*)&_robot2d_geom, sizeof(_robot2d_geom), NULL);
    return(0);
  }
  else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                                PLAYER_IR_POSE,
                                ir_id))
  {
    /* Return the ir pose info */
    Publish( ir_id, resp_queue,
	     PLAYER_MSGTYPE_RESP_ACK,
	     hdr->subtype,
	     (void*)&_ir_geom, sizeof( _ir_geom ), NULL);
    return(0);
  }
  else
  {
    PLAYER_WARN("unknown config request to wbr914 driver");
    return(-1);
  }
}

int wbr914::HandleCommand(player_msghdr * hdr, void* data)
{
  if(Message::MatchMessage(hdr,
                           PLAYER_MSGTYPE_CMD,
                           PLAYER_POSITION2D_CMD_VEL,
                           this->position_id))
  {
    HandleVelocityCommand( (player_position2d_cmd_vel_t*)data );
    UpdateM3();
    return(0);
  }
  else if ( Message::MatchMessage(hdr,
				  PLAYER_MSGTYPE_CMD,
				  PLAYER_DIO_CMD_VALUES,
				  this->dio_id))
  {
    HandleDigitalOutCommand( (player_dio_cmd_t*)data );
    return(0);
  }


  return(-1);
}

void wbr914::HandleVelocityCommand(player_position2d_cmd_vel_t* velcmd)
{
  // need to calculate the left and right velocities
  int32_t transvel = MPS2Vel( velcmd->vel.px );
  int32_t rotvel = MPS2Vel( velcmd->vel.pa * DEFAULT_AXLE_LENGTH/2.0 );
  int32_t leftvel = transvel - rotvel;
  int32_t rightvel = transvel + rotvel;

  //  printf( "VelCmd: px=%1.3f, pa=%1.3f, trvel=%d, rotvel=%d, rvel=%d, lvel=%d\n", velcmd->vel.px, velcmd->vel.pa, transvel, rotvel, leftvel, rightvel );
  SetContourMode( VelocityContouringProfile );

  // now we set the speed
  if ( this->_motorsEnabled )
  {
    SetVelocityInTicks( leftvel, rightvel );
  }
  else
  {
    SetVelocityInTicks( 0, 0 );
    printf( "Motors not enabled\n" );
  }
}

void wbr914::HandleDigitalOutCommand( player_dio_cmd_t* doutCmd )
{
  SetDigitalData( doutCmd );
}

void wbr914::GetAllData( void )
{
  // Don't bother reading them if nobody is subscribed to them
  if ( position_subscriptions )
  {
    GetPositionData( &_data.position );
  }
  
  if ( ir_subscriptions )
  {
    GetIRData( &_data.ir );
  }

  if ( aio_subscriptions )
  {
    GetAnalogData( &_data.aio );
  }

  if ( dio_subscriptions )
  {
    GetDigitalData( &_data.dio );
  }
}

void wbr914::GetPositionData( player_position2d_data_t* d )
{
  // calculate position data
  int32_t left_pos;
  int32_t right_pos;

  GetPositionInTicks( &left_pos, &right_pos );

  int32_t change_left  = left_pos - last_lpos;
  int32_t change_right = right_pos - last_rpos;

  last_lpos = left_pos;
  last_rpos = right_pos;

  // Calculate translational and rotational change
  // translational change = avg of both changes
  // rotational change is half the diff between both changes
  double transchange = Ticks2Meters( (change_left + change_right)>>1 );
  double rotchange = Ticks2Meters( (change_left - change_right)>>1 );

  double dx,dy,Theta;
  // Radius of the wheels
  double r = WHEEL_RADIUS;

  // No translation
  if (transchange == 0)
  {
    // Theta = 2pi rads * rotchange/ (2pi * wheel radius)
    Theta = rotchange/r;
    dx = dy= 0;
  }
  // No rotation
  else if (rotchange == 0)
  {
    dx = transchange;
    dy = 0;
    Theta = 0;
  }
  // Translation and rotation
  else
  {
    Theta = rotchange/r;
    double R = transchange * r / rotchange;
    dy = R - R*cos(Theta);
    dx = R*sin(Theta);
  }

  // add code to read in the speed data
  int32_t left_vel, right_vel;

  GetVelocityInTicks( &left_vel, &right_vel );

  double lv = Vel2MPS( left_vel );
  double rv = Vel2MPS( right_vel );
  double trans_vel = 100 * (lv + rv)/2;
  double rot_vel = (lv - rv)/2;
  double rot_vel_rad = 100 * rot_vel/(r);

  // now write data
  double rad_Theta = DTOR(yaw);
  x+=(dx*cos(rad_Theta) + dy*sin(rad_Theta));
  y+=(dy*cos(rad_Theta) + dx*sin(rad_Theta));

  yaw += Theta;

  while (yaw > 2*M_PI)
    yaw -= 2*M_PI;

  while (yaw < 0)
    yaw += 2*M_PI;

  d->pos.px = x;
  d->pos.py = y;
  d->pos.pa = yaw;
  d->vel.px = trans_vel;
  d->vel.pa = rot_vel_rad;
}

/* this will update the IR part of the client data
 * returns:
 */
void wbr914::GetIRData(player_ir_data_t * d)
{
  // At 80cm Vmin=0.25V Vtyp=0.4V Vmax=0.55V
  // At 10cm delta increase in voltage Vmin=1.75V Vtyp=2.0V Vmax=2.25V
  // Therefore lets choose V=0.25V at 80cm and V=2.8V (2.25+0.55) at 10cm
  // Assume the formula for mm = 270 * (voltage)^-1.1 for 80cm to 10cm
  // Assume ADC input of 5.0V gives max value of 1023

  float adcLo = 0.0;
  float adcHi = 5.0;
  float vPerCount = (adcHi-adcLo)/1023.0;
  //  float v80 = 0.25;
  //  float deltaV = 2.25;
  //  float v10 = v80+deltaV;
  //  float mmPerVolt = (800.0-100.0)/(v80-v10); 

  d->voltages_count = NUM_IR_SENSORS;
  d->ranges_count = d->voltages_count;

  for (uint32_t i=0; i < d->ranges_count; i++)
  {
    int16_t val = 0;

    GetAnalogSensor( i+8, &val );
    float voltage = (float)val*vPerCount;
    d->voltages[ i ] = voltage;

    // Range values are useless further out than 80-90 cm
    // with the Sharp sensors, so truncate them accordingly
    if ( val < 80 )
    {
      val = 80;
    }

    // Convert 10 bit value to a distance in meters
    float meters;

    // Formula for range conversion is different for long range
    // sensors than short range ones. Use appropriate formula.
    if ( i == 5 || i == 7 )
    {
      // Beak side firing sensors are longer range sensors
      // Sharp GP2Y0A02 sensors 20-150cm
      meters = ((16933.0/((float)val - 8.0)) - 13.0)/100.0;
    }
    else
    {
      // Sharp GP2Y0A21 sensors 10-80cm
      meters = ((6787.0/((float)val - 3.0)) - 4.0)/100.0;
    }
    d->ranges[ i ] = meters;
  }
}

/*
  Update the Analog input part of the client data

  We cannot reliably detect whether there is an I/O
  board attached to the M3 so blindly return the data.
 */
void wbr914::GetAnalogData(player_aio_data_t * d)
{
  // Read the 8 analog inputs on the second I/O board
  d->voltages_count = 8;

  float adcLo = 0.0;
  float adcHi = 5.0;
  float vPerCount = (adcHi-adcLo)/1023.0;

  for (uint32_t i=0; i < d->voltages_count; i++)
  {
    int16_t val = 0;

    GetAnalogSensor( i, &val );
    float voltage = (float)val*vPerCount;
    d->voltages[ i ] = voltage;
  }
}

/*
  Update the Digital input part of the client data

  We cannot reliably detect whether there is an I/O
  board attached to the M3 so blindly return the data.
 */
void wbr914::GetDigitalData(player_dio_data_t * d)
{
  // Read the 16 digital inputs
  uint16_t din;

  d->count = 16;
  GetDigitalIn( &din );

  // Byte flip the data to make the Input from the
  // optional I/O board show up in the upper byte.
  d->digin = (uint32_t)( (din>>8) | (din<<8));
}

/*
  Set the Digital outputs on the robot

  We cannot reliably detect whether there is an I/O
  board attached to the M3 so blindly set the data.
 */
void wbr914::SetDigitalData( player_dio_cmd_t * d )
{
  // We only have 16 bits of Dig out, so strip extra bits
  uint16_t data = d->digout & 0xFFFF;

  // Different number of digital bits being requested to
  // be set than we must actually set in the hardware.
  // Handle by using part of the last sent data.
  if ( d->count < 16 )
  {
    // Keep the last dig out bits that have not changed
    uint16_t mask = (0xffff << d->count);
    uint16_t oldPart = _lastDigOut & mask;

    // Invert the mask and keep the bits that are to change.
    mask = mask ^ 0xFFFF;
    data &= mask;

    // Build the output data
    data |= oldPart;
  }

  _lastDigOut = data;

  // Byte flip the data to make the Output to from the
  // optional I/O board show up in the upper byte.
  data = ( (data>>8) | (data<<8) );

  // Always set 16 bits of data
  SetDigitalOut( data );
}

//-------------------------------------------------------

// TODO: Make the bogusRC temporally stable and threadsafe
const char* wbr914::GetPMDErrorString( int rc )
{
  static const char* errorStrings[] =
  {
    "Success",
    "Reset",
    "Invalid Instruction",
    "Invalid Axis",
    "Invalid Parameter",
    "Trace Running",
    "Flash",
    "Block Out of Bounds",
    "Trace buffer zero",
    "Bad checksum",
    "Not primary port",
    "Invalid negative value",
    "Invalid parameter change",
    "Limit event pending",
    "Invalid move into limit"
  };
  static char bogusRC[ 80 ];

  if ( rc < (int)sizeof( errorStrings ) )
  {
    return errorStrings[ rc ];
  }

  sprintf( bogusRC, "Unknown error %d", rc );
  return bogusRC;
}

int wbr914::ResetRawPositions()
{
  if ( _debug )
    printf("Reset Odometry\n");
  int Values[2];
  Values[0] = 0;
  Values[1] = 0;

  if ( _debug )
    printf( "SetActualPositionInTicks\n" );
  SetActualPositionInTicks( 0, 0 );
  UpdateM3();

  last_lpos = 0;
  last_rpos = 0;
  /*
  player_position2d_data_t data;
  memset(&data,0,sizeof(player_position2d_data_t));
  Publish( position_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, &data, sizeof(data),NULL);
  */
  x   = 0;
  y   = 0;
  yaw = 0;
  return 0;
}


int wbr914::ReadBuf(unsigned char* s, size_t len)