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
{
printf("DropOff: Lowered!\n");
myWaitingOnLower = false;
myWaitingOnRaised = true;
myStartRaised = true;
}
myLastMoved.setToNow();
myLastPose = myRobot->getPose();
}
if (myWaitingOnRaised)
{
if (myStartRaised)
{
printf("DropOff: Raising gripper a little bit\n");
myGripper->liftCarry(15);
mySentGripper.setToNow();
myStartRaised = false;
}
if (mySentGripper.mSecSince() > 1000)
{
printf("DropOff: Raised the gripper a little bit\n");
myWaitingOnRaised = false;
myLowered = true;
}
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
myLastMoved.setToNow();
myLastPose = myRobot->getPose();
return &myDesired;
}
if (myLowered)
{
if (!myTried)
{
printf("DropOff: Trying to let go of the block.\n");
myTriedStart.setToNow();
myTried = true;
}
myGripper->gripOpen();
if (myGripper->getGripState() == 1 || myTriedStart.mSecSince() > 3000)
{
printf("DropOff: Succeeded, dropped off the block.\n");
myGripper->liftUp();
myState = STATE_SUCCEEDED;
}
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
return &myDesired;
}
pose = myRobot->getPose();
dist = myLastPose.findDistanceTo(pose);
if (dist < 5 && myLastMoved.mSecSince() > 2500)
{
printf("DropOff: Failed, no movement in the last 2500 msec.\n");
myState = STATE_FAILED;
myGripper->gripOpen();
myGripper->liftUp();
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
return &myDesired;
}
else if (dist > 5)
{
myLastMoved.setToNow();
myLastPose = myRobot->getPose();
}
if (!blobSeen)
{
if (((!myPointedDown && myLastSeen.mSecSince() > 1500) ||
(myPointedDown && myLastSeen.mSecSince() > 4000)) &&
myGripper->getBreakBeamState() == 0)
{
printf("DropOff: Lost the blob, failed, last saw it %ld msec ago.\n",
myLastSeen.mSecSince());
myState = STATE_FAILED;
myGripper->gripOpen();
ArUtil::sleep(3);
myGripper->liftUp();
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
return &myDesired;
}
}
else
myLastSeen.setToNow();
if (blobSeen)
{
xRel = (double)(blob.getXCG() - WIDTH/2.0) / (double)WIDTH + CAMERA_X_CORRECTION;
yRel = -(double)(blob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
//printf("xRel %.3f yRel %.3f: ", xRel, yRel);
}
else
{
//printf("No blob: ");
}
if (blobSeen && yRel < -.2 && !myPointedDown)
{
printf("DropOff: Pointing the camera down!!!\n");
myAMPTU->panTilt(0, -75);
myPointedDown = true;
}
if (!blobSeen || ArMath::fabs(xRel) < .001)
{
//printf("Going straight ahead\n");
myDesired.setDeltaHeading(0);
}
else
{
//printf("Turning %.2f\n", -xRel * 30);
if (-xRel > 1.0)
myDesired.setDeltaHeading(30);
else if (-xRel < -1.0)
myDesired.setDeltaHeading(-30);
else
myDesired.setDeltaHeading(-xRel * 30);
}
if (myRobot->isLeftTableSensingIRTriggered() ||
myRobot->isRightTableSensingIRTriggered())
myDesired.setVel(50);
else
myDesired.setVel(100);
return &myDesired;
}
