C++ ArGripper::liftDown方法代码示例

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
//.........这里部分代码省略.........

//.........这里部分代码省略.........
      myPickUp->deactivate();
      myDriveTo->activate();
      myDriveTo->setChannel(myDropWall);
      myBackup->deactivate();
    }
    if (myGripper->getGripState() != 2 || 
	myGripper->getBreakBeamState() == 0)
    {
      printf("###### DriveToDropWall:: failed (lost cube)\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    if (myDriveTo->getState() == DriveTo::STATE_FAILED)
    {
      printf("###### DriveToDropWall: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
    {
      printf("###### DriveToDropWall: succesful\n");
      setState(STATE_DROP);
      //handler();
      return;
    }
    break;
  case STATE_DROP:
    if (myNewState)
    {
      printf("!! Drop\n");
      printf("@@@@@ Drop lowering lift\n");
      myGripper->liftDown(); 
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
      myGripOpenSent = false;
    }
    
    myGripper->liftDown();
    if ((myStateStart.mSecSince() > 500 && 
	 myGripper->isLiftMaxed() && myStateStart.mSecSince() < 5000) ||
	myStateStart.mSecSince() > 5000)
    {
      myGripper->gripOpen();
      /*if (!myGripOpenSent)
      {
	ArUtil::sleep(3);
	myGripper->gripOpen();
      }
      myGripOpenSent = true;
      */
    }
    if (myGripper->getGripState() == 1 || myStateStart.mSecSince() > 6000)
    {
      printf("###### Drop: success\n");
      setState(STATE_DROP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_DROP_BACKUP:
    if (myNewState)

//.........这里部分代码省略.........
      handler();
      return;
    }
    else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
    {
      printf("## PickUpBlock2: successful\n");
      setState(STATE_ACQUIRE_WALL);
      myGripper->liftUp();
      handler();
      return;
    }
    break;
  case STATE_ACQUIRE_WALL:
    if (myNewState)
    {
      myNewState = false;
      mySony->panTilt(0, -5);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_FIRST_WALL);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED)
    {
      printf("## AcquireWall:: failed\n");
      setState(STATE_FAILED);
      handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("## AcquireWall: successful\n");
      setState(STATE_DRIVETO_WALL);
      handler();
      return;
    }
    break;
  case STATE_DRIVETO_WALL:
    if (myNewState)
    {
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->activate();
      myDriveTo->setChannel(COLOR_FIRST_WALL);
      myBackup->deactivate();
    }
    if (myDriveTo->getState() == DriveTo::STATE_FAILED)
    {
      printf("## DriveToWall: failed\n");
      setState(STATE_FAILED);
      handler();
      return;
    }
    else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
    {
      printf("## DriveToWall: succesful\n");
      setState(STATE_DROP);
      handler();
      return;
    }
    break;
  case STATE_DROP:
    if (myNewState)
    {
      myGripper->liftDown(); 
      myNewState = false;
    }
    
    if (myStateStart.mSecSince() > 3500)
    {
      myGripper->gripOpen();
    }
    if (myStateStart.mSecSince() > 5500)
    {
      printf("## Drop: success\n");
      setState(STATE_SUCCEEDED);
      handler();
      return;
    }
    break;
  case STATE_SUCCEEDED:
    printf("Succeeded at the task!\n");
    Aria::shutdown();
    myRobot->disconnect();
    myRobot->stopRunning();
    return;
  case STATE_FAILED:
    printf("Failed to complete the task!\n");
    Aria::shutdown();
    myRobot->disconnect();
    myRobot->stopRunning();
    return;
  default:
    printf("TakeBlockToWall::handler: Unknown state!\n");
    
  }

}

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

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

  // 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)
  {
    myGripper->gripOpen();
    ArUtil::sleep(3);
    myGripper->liftDown();
    mySentLiftDown.setToNow();
    myPointedDown = false;
    myState = STATE_LOOKING;
    myLastSeen.setToNow();
    myTried = false;
    myLastMoved.setToNow();
    myLastPose = myRobot->getPose();
    myWaitingOnGripper = true;
    printf("@@@@@ Pickup: Lowering lift\n");
  }

  // we want to sit still until the lift is down or for a second and a half
  if (myWaitingOnGripper) 
  {
    if (mySentLiftDown.mSecSince() < 500 || 
	((!myGripper->isLiftMaxed() || myGripper->getGripState() != 2)
	 && mySentLiftDown.mSecSince() < 5000))
    {
      myGripper->liftDown();
      myGripper->gripOpen();
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      myLastMoved.setToNow();
      myLastPose = myRobot->getPose();
      return &myDesired;
    }
    else
    {
      myWaitingOnGripper = false;
    }
    myLastMoved.setToNow();
    myLastPose = myRobot->getPose();
  } 

  if (myGripper->getBreakBeamState() != 0)
  {
    if (myGripper->getGripState() == 2)
    {
      printf("PickUp: Succeeded, have block.\n");
      myGripper->liftUp();
      myState = STATE_SUCCEEDED;
    }
    else if (!myTried)
    {
      myGripper->gripClose();
      printf("PickUp: Trying to pick up.\n");
      myTried = true;
      myTriedStart.setToNow();
    }
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
  // this means that the grippers are closed, but we don't have anything in 
  // them, ie that we failed to get the block
  else if (myTried && (myGripper->getGripState() == 2 || 
		       myTriedStart.mSecSince() > 5000))
  {
    myState = STATE_FAILED;
    myGripper->gripOpen();
    ArUtil::sleep(3);
    myGripper->liftUp();
    printf("PickUp: Grippers closed, didn't get a block, failed.\n");
    myDesired.setVel(0);
    myDesired.setDeltaHeading(0);
    return &myDesired;
  }
     
  pose = myRobot->getPose();
//.........这里部分代码省略.........

 void liftDown()
 {
   ArLog::log(ArLog::Normal, "Moving gripper lift down...");
   myGripper->liftDown();
 }