ArActionDesired *Acquire::fire(ArActionDesired currentDesired) { myDesired.reset(); switch (myState) { case STATE_START_LOOKING: myFirstTurn.clear(); mySecondTurn.clear(); myState = STATE_LOOKING; case STATE_LOOKING: 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: return NULL; } }
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; } }