segwayrmp.cc

机器人仿真软件

{
  switch(subtype) 
  {
    case PLAYER_POSITION3D_MOTOR_POWER:
      // just set a flag telling us whether we should
      // act on motor commands
      // set the commands to 0... think it will automatically
      // do this for us.  
      if(((char*)buffer)[0]) 
        this->motor_allow_enable = true;
      else
        this->motor_allow_enable = false;

      printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable);

      Publish(this->position3d_id, client, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION3D_MOTOR_POWER);
      return 0;

    default:
      printf("segwayrmp received unknown config request %d\n", 
             subtype);
  }

  // return -1, to indicate that we did process the message
  return(-1);
}


int
SegwayRMP::Read()
{
  CanPacket pkt;
  int channel;
  int ret;
  rmp_frame_t data_frame[2];
  
  //static struct timeval last;
  //struct timeval curr;

  data_frame[0].ready = 0;
  data_frame[1].ready = 0;

  // read one cycle of data from each channel
  for(channel = 0; channel < DUALCAN_NR_CHANNELS; channel++) 
  {
    ret=0;
    // read until we've gotten all 5 packets for this cycle on this channel
    while((ret = canio->ReadPacket(&pkt, channel)) >= 0)
    {
      // then we got a new packet...

      //printf("SEGWAYIO: pkt: %s\n", pkt.toString());

      data_frame[channel].AddPacket(pkt);

      // if data has been filled in, then let's make it the latest data 
      // available to player...
      if(data_frame[channel].IsReady()) 
      {
        // Only bother doing the data conversions for one channel.
        // It appears that the data on channel 0 is garbarge, so we'll read
        // from channel 1.
        if(channel == 1)
        {
	  UpdateData(&data_frame[channel]);
        }

        data_frame[channel].ready = 0;
        break;
      }
    }

    if (ret < 0) 
    {
      PLAYER_ERROR2("error (%d) reading packet on channel %d",
                    ret, channel);
    }
  }

  return(0);
}

void
SegwayRMP::UpdateData(rmp_frame_t * data_frame)
{
  int delta_lin_raw, delta_ang_raw;
  double delta_lin, delta_ang;
  double tmp;

  // Get the new linear and angular encoder values and compute
  // odometry.  Note that we do the same thing here, regardless of 
  // whether we're presenting 2D or 3D position info.
  delta_lin_raw = Diff(this->last_raw_foreaft,
                       data_frame->foreaft,
                       this->firstread);
  this->last_raw_foreaft = data_frame->foreaft;
  
  delta_ang_raw = Diff(this->last_raw_yaw,
                       data_frame->yaw,
                       this->firstread);
  this->last_raw_yaw = data_frame->yaw;
  
  delta_lin = (double)delta_lin_raw / (double)RMP_COUNT_PER_M;
  delta_ang = DTOR((double)delta_ang_raw / 
                   (double)RMP_COUNT_PER_REV * 360.0);
  
  // First-order odometry integration
  this->odom_x += delta_lin * cos(this->odom_yaw);
  this->odom_y += delta_lin * sin(this->odom_yaw);
  this->odom_yaw += delta_ang;
  
  // Normalize yaw in [0, 360]
  this->odom_yaw = atan2(sin(this->odom_yaw), cos(this->odom_yaw));
  if (this->odom_yaw < 0)
    this->odom_yaw += 2 * M_PI;
  
  // first, do 2D info.
  this->position_data.pos.px = this->odom_x;
  this->position_data.pos.py = this->odom_y;
  this->position_data.pos.pa = this->odom_yaw;
  
  // combine left and right wheel velocity to get foreward velocity
  // change from counts/s into mm/s
  this->position_data.vel.px = ((double)data_frame->left_dot +
                          (double)data_frame->right_dot) /
                         (double)RMP_COUNT_PER_M_PER_S 
                         / 2.0;
  
  // no side speeds for this bot
  this->position_data.vel.py = 0;
  
  // from counts/sec into deg/sec.  also, take the additive
  // inverse, since the RMP reports clockwise angular velocity as
  // positive.
  this->position_data.vel.pa = 
          DTOR(-(double)data_frame->yaw_dot / (double)RMP_COUNT_PER_DEG_PER_S);
  
  this->position_data.stall = 0;
  
  // now, do 3D info.
  this->position3d_data.pos.px = this->odom_x;
  this->position3d_data.pos.py = this->odom_y;
  // this robot doesn't fly
  this->position3d_data.pos.pz = 0;

  /// TODO left off here
  
  // normalize angles to [0,360]
  tmp = NORMALIZE(DTOR((double)data_frame->roll /
                       (double)RMP_COUNT_PER_DEG));
  if(tmp < 0)
    tmp += 2*M_PI;
  this->position3d_data.pos.proll = tmp;//htonl((int32_t)rint(tmp * 1000.0));
  
  // normalize angles to [0,360]
  tmp = NORMALIZE(DTOR((double)data_frame->pitch /
                       (double)RMP_COUNT_PER_DEG));
  if(tmp < 0)
    tmp += 2*M_PI;
  this->position3d_data.pos.ppitch = tmp;//htonl((int32_t)rint(tmp * 1000.0));
  
  this->position3d_data.pos.pyaw = tmp;//htonl(((int32_t)(this->odom_yaw * 1000.0)));
  
  // combine left and right wheel velocity to get foreward velocity
  // change from counts/s into m/s
  this->position3d_data.vel.px = 
    ((double)data_frame->left_dot +
                          (double)data_frame->right_dot) /
                         (double)RMP_COUNT_PER_M_PER_S 
                          / 2.0;
  // no side or vertical speeds for this bot
  this->position3d_data.vel.py = 0;
  this->position3d_data.vel.pz = 0;
  
  this->position3d_data.vel.proll = 
    (double)data_frame->roll_dot /
                        (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180;
  this->position3d_data.vel.ppitch = 
    (double)data_frame->pitch_dot /
                        (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180;
  // from counts/sec into millirad/sec.  also, take the additive
  // inverse, since the RMP reports clockwise angular velocity as
  // positive.

  // This one uses left_dot and right_dot, which comes from odometry
  this->position3d_data.vel.pyaw = 
    (double)(data_frame->right_dot - data_frame->left_dot) /
                         (RMP_COUNT_PER_M_PER_S * RMP_GEOM_WHEEL_SEP * M_PI);
  // This one uses yaw_dot, which comes from the IMU
  //data.position3d_data.yawspeed = 
  //  htonl((int32_t)(-rint((double)data_frame->yaw_dot / 
  //                        (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180 * 1000)));
  
  this->position3d_data.stall = 0;
  
  // fill in power data.  the RMP returns a percentage of full,
  // and the specs for the HT say that it's a 72 volt system.  assuming
  // that the RMP is the same, we'll convert to decivolts for Player.
  this->power_data.volts = 
    data_frame->battery * 72;
  
  firstread = false;  
}  


int
SegwayRMP::Write(CanPacket& pkt)
{
  return(canio->WritePacket(pkt));
}

/* Creates a status CAN packet from the given arguments
 */  
void
SegwayRMP::MakeStatusCommand(CanPacket* pkt, uint16_t cmd, uint16_t val)
{
  int16_t trans,rot;

  pkt->id = RMP_CAN_ID_COMMAND;
  pkt->PutSlot(2, cmd);
 
  // it was noted in the windows demo code that they
  // copied the 8-bit value into both bytes like this
  pkt->PutByte(6, val);
  pkt->PutByte(7, val);

  trans = (int16_t) rint((double)this->curr_xspeed * 
                         (double)RMP_COUNT_PER_MM_PER_S);

  if(trans > RMP_MAX_TRANS_VEL_COUNT)
    trans = RMP_MAX_TRANS_VEL_COUNT;
  else if(trans < -RMP_MAX_TRANS_VEL_COUNT)
    trans = -RMP_MAX_TRANS_VEL_COUNT;

  rot = (int16_t) rint((double)this->curr_yawspeed * 
                       (double)RMP_COUNT_PER_DEG_PER_SS);

  if(rot > RMP_MAX_ROT_VEL_COUNT)
    rot = RMP_MAX_ROT_VEL_COUNT;
  else if(rot < -RMP_MAX_ROT_VEL_COUNT)
    rot = -RMP_MAX_ROT_VEL_COUNT;

  // put in the last speed commands as well
  pkt->PutSlot(0,(uint16_t)trans);
  pkt->PutSlot(1,(uint16_t)rot);
  
  if(cmd) 
  {
    printf("SEGWAYIO: STATUS: cmd: %02x val: %02x pkt: %s\n", 
	   cmd, val, pkt->toString());
  }
}

/* takes a player command (in host byte order) and turns it into CAN packets 
 * for the RMP 
 */
void
SegwayRMP::MakeVelocityCommand(CanPacket* pkt, 
                               int32_t xspeed, 
                               int32_t yawspeed)
{
  pkt->id = RMP_CAN_ID_COMMAND;
  pkt->PutSlot(2, (uint16_t)RMP_CAN_CMD_NONE);
  
  // we only care about cmd.xspeed and cmd.yawspeed
  // translational velocity is given to RMP in counts 
  // [-1176,1176] ([-8mph,8mph])

  // player is mm/s
  // 8mph is 3576.32 mm/s
  // so then mm/s -> counts = (1176/3576.32) = 0.32882963

  if(xspeed > this->max_xspeed)
  {
    PLAYER_WARN2("xspeed thresholded! (%d > %d)", xspeed, this->max_xspeed);
    xspeed = this->max_xspeed;
  }
  else if(xspeed < -this->max_xspeed)
  {
    PLAYER_WARN2("xspeed thresholded! (%d < %d)", xspeed, -this->max_xspeed);
    xspeed = -this->max_xspeed;
  }

  this->curr_xspeed = xspeed;

  int16_t trans = (int16_t) rint((double)xspeed * 
                                 (double)RMP_COUNT_PER_MM_PER_S);

  if(trans > RMP_MAX_TRANS_VEL_COUNT)
    trans = RMP_MAX_TRANS_VEL_COUNT;
  else if(trans < -RMP_MAX_TRANS_VEL_COUNT)
    trans = -RMP_MAX_TRANS_VEL_COUNT;

  if(yawspeed > this->max_yawspeed)
  {
    PLAYER_WARN2("yawspeed thresholded! (%d > %d)", 
                 yawspeed, this->max_yawspeed);
    yawspeed = this->max_yawspeed;
  }
  else if(yawspeed < -this->max_yawspeed)
  {
    PLAYER_WARN2("yawspeed thresholded! (%d < %d)", 
                 yawspeed, -this->max_yawspeed);
    yawspeed = -this->max_yawspeed;
  }
  this->curr_yawspeed = yawspeed;

  // rotational RMP command \in [-1024, 1024]
  // this is ripped from rmi_demo... to go from deg/s to counts
  // deg/s -> count = 1/0.013805056
  int16_t rot = (int16_t) rint((double)yawspeed * 
                               (double)RMP_COUNT_PER_DEG_PER_S);

  if(rot > RMP_MAX_ROT_VEL_COUNT)
    rot = RMP_MAX_ROT_VEL_COUNT;
  else if(rot < -RMP_MAX_ROT_VEL_COUNT)
    rot = -RMP_MAX_ROT_VEL_COUNT;

  pkt->PutSlot(0, (uint16_t)trans);
  pkt->PutSlot(1, (uint16_t)rot);
}

void
SegwayRMP::MakeShutdownCommand(CanPacket* pkt)
{
  pkt->id = RMP_CAN_ID_SHUTDOWN;

  printf("SEGWAYIO: SHUTDOWN: pkt: %s\n",
	 pkt->toString());
}

// Calculate the difference between two raw counter values, taking care
// of rollover.
int
SegwayRMP::Diff(uint32_t from, uint32_t to, bool first)
{
  int diff1, diff2;
  static uint32_t max = (uint32_t)pow(2,32)-1;

  // if this is the first time, report no change
  if(first)
    return(0);

  diff1 = to - from;

  /* find difference in two directions and pick shortest */
  if(to > from)
    diff2 = -(from + max - to);
  else 
    diff2 = max - from + to;

  if(abs(diff1) < abs(diff2)) 
    return(diff1);
  else
    return(diff2);
}