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;
}
}
