C++ ArActionDesired::setVel方法代码示例

ArActionDesired *JoydriveAction::fire(ArActionDesired currentDesired)
{
  int rot, trans;

  printf("%6ld ms since last loop. ms longer than desired:  %6ld.  mpac %d\n",
	 lastLoopTime.mSecSince(), 
	 lastLoopTime.mSecSince() - loopTime, myRobot->getMotorPacCount());
  lastLoopTime.setToNow();

  if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
				    myJoyHandler.getButton(2)))
  {
    if (ArMath::fabs(myRobot->getVel()) < 10.0)
      myRobot->comInt(ArCommands::ENABLE, 1);
    myJoyHandler.getAdjusted(&rot, &trans);
    myDesired.setVel(trans);
    myDesired.setDeltaHeading(-rot);
    return &myDesired;
  }
  else
  {
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
}

// the guts of the thing
ArActionDesired *JoydriveAction::fire(ArActionDesired currentDesired)
{
  int rot, trans;

  // print out some info about hte robot
  printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d", myRobot->getX(),
	 myRobot->getY(), myRobot->getTh(), myRobot->getVel(), 
	 myRobot->getMotorPacCount());
  fflush(stdout);

  // see if one of the buttons is pushed, if so drive
  if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
				    myJoyHandler.getButton(2)))
  {
    // get the readings from the joystick
    myJoyHandler.getAdjusted(&rot, &trans);
    // set what we want to do
    myDesired.setVel(trans);
    myDesired.setDeltaHeading(-rot);
    // return the actionDesired
    return &myDesired;
  }
  else
  {
    // set what we want to do
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    // return the actionDesired
    return &myDesired;
  }
}

ArActionDesired *Acquire::fire(ArActionDesired currentDesired)
{
  myDesired.reset();
  myDesired.setVel(0);
  switch (myState) {
  case STATE_START_LOOKING:
    myFirstTurn.clear();
    mySecondTurn.clear();
    myState = STATE_LOOKING;
    myGripper->liftUp();
    myGripper->gripClose();
    printf("Acquire: Raising lift\n");
    myStartUp.setToNow();
    myTryingGripper = true;
  case STATE_LOOKING:
    if (myTryingGripper && (myStartUp.mSecSince() < 600 ||
			    ((!myGripper->isLiftMaxed() ||
			      myGripper->getGripState() != 2) && 
			     myStartUp.mSecSince() < 5000)))
    {
      myGripper->liftUp();
      myGripper->gripClose();
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      return &myDesired;
    } 
    else if (myTryingGripper)
    {
      printf("Acquire: Done raising lift %ld after started.\n", 
	     myStartUp.mSecSince());
      myTryingGripper = false;
    }
    if (myActs->getNumBlobs(myChannel) > 0)
    {
      myDesired.setDeltaHeading(0);
      myState = STATE_SUCCEEDED;
      printf("Acquire: Succeeded!\n");
    }
    else if (myFirstTurn.didAll() && mySecondTurn.didAll())
    {
      myDesired.setDeltaHeading(0);
      myState = STATE_FAILED;
      printf("Acquire: Did two revolutions, didn't see the blob, Failed!\n");
    } 
    else
    {
      myFirstTurn.update(myRobot->getTh());
      if (myFirstTurn.didAll())
	mySecondTurn.update(myRobot->getTh());
      myDesired.setDeltaHeading(8);
    }
    return &myDesired;
  default:
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
}

inline ArActionDesired *ArActionFollowTarget::fire(ArActionDesired currentDesired)
{
  double tempvel;
  //double temprotvel;

  // reset the actionDesired (must be done), to clear
  // its previous values.
  //myDesired.reset();

  if(m_rel_distance > dnear)
  {
    //target is safe
    if(m_rel_distance < dclose)
    {
    //target is close
    tempvel = calcvclose();
    }
    else
    {
      //target is far
    tempvel = calcvfar();
    }
  }
  else
  {
    //brake
    tempvel = 0;
  }

  tempvel = (tempvel > m_speed_limit)? (m_speed_limit) : (tempvel) ;

  double absoffset = ::fabs(m_rel_offset);

  if ( absoffset  > th_near)
  {
    if(absoffset  < th_close )
    {
      //myDesired.setRotVel();
      double numer = (m_rel_offset - th_near)*(m_rel_offset - th_near);
      double denom = (th_close - th_near)* (th_close - th_near);
      myDesired.setDeltaHeading (my_sign(m_rel_offset)*deltah_close * (numer/denom) );  
    }
    else
    {
      tempvel *= speed_dump;
      myDesired.setDeltaHeading( my_sign(m_rel_offset)*deltah_max);
    }
  }
  else
  {
    myDesired.setDeltaHeading(0);      
  }

  myDesired.setVel(tempvel);

  printf("Desired: VEL: %f dHeading: %f \n", myDesired.getVel()
    ,myDesired.getDeltaHeading());
  return &myDesired;
}

ArActionDesired *ActionTest::fire(ArActionDesired currentDesired)
{
  myActionDesired.reset();
  if (fabs(mySpeed) > 1)
    myActionDesired.setVel(mySpeed);
  if (fabs(myTurnAmount) > 1)
    myActionDesired.setDeltaHeading(myTurnAmount);
  return &myActionDesired;  
}

/*
  This fire is the whole point of the action.
  currentDesired is the combined desired action from other actions
  previously processed by the action resolver.  In this case, we're
  not interested in that, we will set our desired 
  forward velocity in the myDesired member, and return it.

  Note that myDesired must be a class member, since this method
  will return a pointer to myDesired to the caller. If we had
  declared the desired action as a local variable in this method,
  the pointer we returned would be invalid after this method
  returned.
*/
ArActionDesired *ActionGos::fire(ArActionDesired currentDesired)
{
  double range;
  double speed;

  // reset the actionDesired (must be done), to clear
  // its previous values.
  myDesired.reset();

  // if the sonar is null we can't do anything, so deactivate
  if (mySonar == NULL)
  {
    deactivate();
    return NULL;
  }
  // get the range of the sonar
  range = mySonar->currentReadingPolar(-70, 70) - myRobot->getRobotRadius();
  // if the range is greater than the stop distance, find some speed to go
  if (range > myStopDistance)
  {
    // just an arbitrary speed based on the range
    speed = range * .3;
    // if that speed is greater than our max, cap it
    if (speed > myMaxSpeed)
      speed = myMaxSpeed;
    // now set the velocity
    myDesired.setVel(speed);
  }
  // the range was less than the stop distance, so request stop
  else
  {
    myDesired.setVel(0);
	myFound = 1;
  }
  // return a pointer to the actionDesired to the resolver to make our request
  return &myDesired;
}

ArActionDesired *DropOff::fire(ArActionDesired currentDesired)
{
  ArPose pose;
  ArACTSBlob blob;
  bool blobSeen = false;
  double xRel, yRel;
  double dist;


  myDesired.reset();
  if (myState == STATE_SUCCEEDED)
  {
    printf("DropOff: Succeeded\n");
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
  else if (myState == STATE_FAILED)
  {
    printf("DropOff: Failed\n");
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }

  if (myActs->getNumBlobs(myChannel) > 0 &&
      (blobSeen = myActs->getBlob(myChannel, 1, &blob)))
  {
    myLastSeen.setToNow();
  }

  // this if the stuff we want to do if we're not going to just drive forward
  // and home in on the color, ie the pickup-specific stuff
  if (myState == STATE_START_LOOKING)
  {
    mySentGripper.setToNow();
    myPointedDown = false;
    myState = STATE_LOOKING;
    myLastSeen.setToNow();
    myTried = false;
    myLastMoved.setToNow();
    myLastPose = myRobot->getPose();
    //myWaitingOnGripper = true;
    myWaitingOnLower = false;
    myLowered = false;
    myWaitingOnRaised = false;
    myStartRaised = false;
    printf("@@@@@ DropOff: Starting\n");
  }

  // we want to sit still until the lift is down or for a second and a half
  /*
  if (myWaitingOnGripper) 
  {
    if (mySentGripper.mSecSince() < 500 || 
	(myGripper->getGripState() != 1
	 && mySentGripper.mSecSince() < 4000))
    {
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      myLastMoved.setToNow();
      myLastPose = myRobot->getPose();
      return &myDesired;
    }
    else
    {
      myWaitingOnGripper = false;
    }
    myLastMoved.setToNow();
    myLastPose = myRobot->getPose();
  } 
  */
  //printf("sensors %d %d stall %d %d\n",!(myRobot->getDigIn() & ArUtil::BIT2),
  //!(myRobot->getDigIn() & ArUtil::BIT3),
  //myRobot->isLeftMotorStalled(), myRobot->isRightMotorStalled());
  if ((myRobot->isLeftBreakBeamTriggered() &&
       myRobot->isRightBreakBeamTriggered()) ||
       myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
  {
    if (!myWaitingOnLower && !myLowered && !myWaitingOnRaised)
    {
      myWaitingOnLower = true;
      printf("DropOff: Lowering gripper\n");
      mySentGripper.setToNow();
    }
  }
  if (myWaitingOnLower)
  {
    /// TODO
    if (mySentGripper.mSecSince() < 600 || 
	(!myGripper->isLiftMaxed() && mySentGripper.mSecSince() < 20000))
    {
      myGripper->liftDown();
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      myLastMoved.setToNow();
      myLastPose = myRobot->getPose();
      return &myDesired;
    }
    else
//.........这里部分代码省略.........

ArActionDesired *DriveTo::fire(ArActionDesired currentDesired)
{
  ArACTSBlob blob;
  double xRel, yRel;

  if (myState == STATE_SUCCEEDED || myState == STATE_FAILED)
  {
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }

  if (myState == STATE_START_LOOKING)
  {
    myState = STATE_LOOKING;
    myLastSeen.setToNow();
  }

  if (myActs->getNumBlobs(myChannel) == 0 || 
      !myActs->getBlob(myChannel, 1, &blob))
  {
    if (myLastSeen.mSecSince() > 1000)
    {
      printf("DriveTo:  Lost the blob, failed.\n");
      myState = STATE_FAILED;
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      return &myDesired;
    }
  }
  else
  {
    myLastSeen.setToNow();
  }

  xRel = (double)(blob.getXCG() - WIDTH/2.0) / (double)WIDTH;
  yRel = (double)(blob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
  
  //printf("xRel %.3f yRel %.3f\n", xRel, yRel);

  myDesired.reset();
  // this if the stuff we want to do if we're not going to just drive forward
  // and home in on the color, ie the pickup-specific stuff
  if (currentDesired.getMaxVelStrength() > 0 &&
      currentDesired.getMaxVel() < 125)
  {
    printf("DriveTo:  Close to a wall of some sort, succeeded.\n");
    myState = STATE_SUCCEEDED;
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
  
  if (ArMath::fabs(xRel) < .10)
  {
    //printf("Going straight ahead\n");
    myDesired.setDeltaHeading(0);
  }
  else
  {
    //printf("Turning %.2f\n", -xRel * 10);
    myDesired.setDeltaHeading(-xRel * 10);
  }
  myDesired.setVel(300);
  return &myDesired;
}