// 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();
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 *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;
}
}
ArActionDesired *ArActionTableSensorLimiter::fire(
ArActionDesired currentDesired)
{
myDesired.reset();
// Note the following commented out section will not work with IR's that
// are sent through byte 4 of the IO packet.
// Reference the NewTableSensingIR parameter
/*
printf("%d ", myRobot->getDigIn());
if (!(myRobot->getDigIn() & ArUtil::BIT0))
printf("leftTable ");
if (!(myRobot->getDigIn() & ArUtil::BIT1))
printf("rightTable ");
if (!(myRobot->getDigIn() & ArUtil::BIT3))
printf("leftBREAK ");
if (!(myRobot->getDigIn() & ArUtil::BIT2))
printf("rightBREAK ");
printf("\n");
*/
if (myRobot->isLeftTableSensingIRTriggered() ||
myRobot->isRightTableSensingIRTriggered())
{
myDesired.setMaxVel(0);
return &myDesired;
}
return NULL;
}
ArActionDesired *ActionTest::fire(ArActionDesired currentDesired)
{
myActionDesired.reset();
if (fabs(mySpeed) > 1)
myActionDesired.setVel(mySpeed);
if (fabs(myTurnAmount) > 1)
myActionDesired.setDeltaHeading(myTurnAmount);
return &myActionDesired;
}
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;
}
}
/*
This is the guts of the action.
*/
ArActionDesired *ActionTurn::fire(ArActionDesired currentDesired)
{
double leftRange, rightRange;
// reset the actionDesired (must be done)
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// Get the left readings and right readings off of the sonar
leftRange = (mySonar->currentReadingPolar(0, 100) -
myRobot->getRobotRadius());
rightRange = (mySonar->currentReadingPolar(-100, 0) -
myRobot->getRobotRadius());
// if neither left nor right range is within the turn threshold,
// reset the turning variable and don't turn
if (leftRange > myTurnThreshold && rightRange > myTurnThreshold)
{
myTurning = 0;
myDesired.setRotVel(0);
}
// if we're already turning some direction, keep turning that direction
else if (myTurning)
{
myDesired.setRotVel(myTurnAmount * myTurning);
}
// if we're not turning already, but need to, and left is closer, turn right
// and set the turning variable so we turn the same direction for as long as
// we need to
else if (leftRange < rightRange)
{
myTurning = -1;
myDesired.setRotVel(myTurnAmount * myTurning);
}
// if we're not turning already, but need to, and right is closer, turn left
// and set the turning variable so we turn the same direction for as long as
// we need to
else
{
myTurning = 1;
myDesired.setRotVel(myTurnAmount * myTurning);
}
// return a pointer to the actionDesired, so resolver knows what to do
return &myDesired;
}
/*
This is the guts of the Turn action.
*/
ArActionDesired *ActionTurns::fire(ArActionDesired currentDesired)
{
double leftRange, rightRange;
// reset the actionDesired (must be done)
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// Get the left readings and right readings off of the sonar
leftRange = (mySonar->currentReadingPolar(0, 100) -
myRobot->getRobotRadius());
rightRange = (mySonar->currentReadingPolar(-100, 0) -
myRobot->getRobotRadius());
// si es que el activador esta en cero que resetee el turn y que no se mueva
if (myActivate == 0)
{
myTurning = 0;
myDesired.setDeltaHeading(0);
}
// if we're already turning some direction, keep turning that direction
else if (myTurning)
{
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// Gira a la izquierda
else if (myDirection == 2)
{
myTurning = -1;
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// Gira a la derecha
else if (myDirection == 1)
{
myTurning = 1;
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// return a pointer to the actionDesired, so resolver knows what to do
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;
}
/*
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
{
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;
}
ArActionDesired *DriveTo::fire(ArActionDesired currentDesired)
{
ArACTSBlob blob;
double xRel, yRel;
ArPose pose;
double dist;
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();
myLastPose = myRobot->getPose();
myLastMoved.setToNow();
}
pose = myRobot->getPose();
dist = myLastPose.findDistanceTo(pose);
if (dist < 5 && myLastMoved.mSecSince() > 1500)
{
printf("DriveTo: Failed, no movement in the last 1500 msec.\n");
myState = STATE_FAILED;
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
return &myDesired;
}
else if (dist > 5)
{
myLastMoved.setToNow();
myLastPose = myRobot->getPose();
}
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 + CAMERA_X_CORRECTION;
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 * 20);
if (-xRel > 1.0)
myDesired.setDeltaHeading(20);
else if (-xRel < -1.0)
myDesired.setDeltaHeading(-20);
else
myDesired.setDeltaHeading(-xRel * 20);
}
myDesired.setVel(150);
return &myDesired;
}
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;
}
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();
dist = myLastPose.findDistanceTo(pose);
if (dist < 5 && myLastMoved.mSecSince() > 1500)
{
printf("PickUp: Failed, no movement in the last 1500 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 (myActs->getNumBlobs(myChannel) == 0 ||
!(blobSeen = myActs->getBlob(myChannel, 1, &blob)))
{
if (((!myPointedDown && myLastSeen.mSecSince() > 1500) ||
(myPointedDown && myLastSeen.mSecSince() > 4000)) &&
myGripper->getBreakBeamState() == 0)
{
printf("PickUp: 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;
yRel = (double)(blob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
//printf("xRel %.3f yRel %.3f:\n", xRel, yRel);
}
else
{
//printf("No blob: ");
}
if (blobSeen && yRel < 0.2 && !myPointedDown)
{
printf("PickUp: Pointing the camera down!!!\n");
mySony->panTilt(0, -ArSonyPTZ::MAX_TILT);
myPointedDown = true;
}
if (!blobSeen || ArMath::fabs(xRel) < .10)
{
//printf("Going straight ahead\n");
myDesired.setDeltaHeading(0);
}
else
{
//printf("Turning %.2f\n", -xRel * 10);
if (ArMath::fabs(-xRel * 10) <= 10)
myDesired.setDeltaHeading(-xRel * 10);
else if (-xRel > 0)
myDesired.setDeltaHeading(10);
else
myDesired.setDeltaHeading(-10);
}
myDesired.setVel(150);
return &myDesired;
}
AREXPORT ArActionDesired *ArPriorityResolver::resolve(
ArResolver::ActionMap *actions, ArRobot *robot, bool logActions)
{
ArResolver::ActionMap::reverse_iterator it;
ArAction *action;
ArActionDesired *act;
ArActionDesired averaging;
bool first = true;
int lastPriority=0;
bool printedFirst = true;
int printedLast=0;
if (actions == NULL)
return NULL;
myActionDesired.reset();
averaging.reset();
averaging.startAverage();
for (it = actions->rbegin(); it != actions->rend(); ++it)
{
action = (*it).second;
if (action != NULL && action->isActive())
{
act = action->fire(myActionDesired);
if (robot != NULL && act != NULL)
act->accountForRobotHeading(robot->getTh());
if (first || (*it).first != lastPriority)
{
averaging.endAverage();
myActionDesired.merge(&averaging);
averaging.reset();
averaging.startAverage();
first = false;
lastPriority = (*it).first;
}
averaging.addAverage(act);
if (logActions && act != NULL && act->isAnythingDesired())
{
if (printedFirst || printedLast != (*it).first)
{
ArLog::log(ArLog::Terse, "Priority %d:", (*it).first);
printedLast = (*it).first;
printedFirst = false;
}
ArLog::log(ArLog::Terse, "Action: %s", action->getName());
act->log();
}
}
}
averaging.endAverage();
myActionDesired.merge(&averaging);
/*
printf(
"desired delta %.0f strength %.3f, desired speed %.0f strength %.3f\n",
myActionDesired.getDeltaHeading(), myActionDesired.getHeadingStrength(),
myActionDesired.getVel(), myActionDesired.getVelStrength());
*/
return &myActionDesired;
}
