void Joydrive::center(void)
{
myCam.panTilt(0,0);
myCam.zoom(0);
}
void TakeBlockToWall::handler(void)
{
Color tempColor;
switch (myState)
{
case STATE_START:
setState(STATE_ACQUIRE_BLOCK);
myDropWall = COLOR_FIRST_WALL;
myLapWall = COLOR_SECOND_WALL;
printf("!! Started state handling!\n");
//handler();
return;
break;
case STATE_ACQUIRE_BLOCK:
if (myNewState)
{
printf("!! Acquire block\n");
myNewState = false;
mySony->panTilt(0, -10);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (have cube?)\n");
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
else if (myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (cube in gripper?)\n");
setState(STATE_PICKUP_BLOCK);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStart.mSecSince() > 35000)
{
printf("###### AcqiureBlock: failed\n");
setState(STATE_BACKUP);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireBlock: successful\n");
setState(STATE_PICKUP_BLOCK);
//handler();
return;
}
break;
case STATE_PICKUP_BLOCK:
if (myNewState)
{
printf("!! Pickup block\n");
myNewState = false;
mySony->panTilt(0, -15);
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("###### PickUpBlock: failed\n");
setState(STATE_BACKUP);
//handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("###### PickUpBlock: successful\n");
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
break;
case STATE_BACKUP:
if (myNewState)
{
printf("!! Backup\n");
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->activate();
myNewState = false;
}
if (myStateStart.mSecSince() > 2000)
{
printf("###### Backup: done\n");
setState(STATE_ACQUIRE_BLOCK2);
//handler();
return;
}
break;
case STATE_ACQUIRE_BLOCK2:
if (myNewState)
{
printf("!! Acquire block 2\n");
myNewState = false;
mySony->panTilt(0, -25);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (have cube?)\n");
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
else if (myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (cube in gripper?)\n");
setState(STATE_PICKUP_BLOCK2);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStart.mSecSince() > 35000)
{
printf("###### AcqiureBlock2: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireBlock2: successful\n");
setState(STATE_PICKUP_BLOCK2);
//handler();
return;
}
break;
case STATE_PICKUP_BLOCK2:
if (myNewState)
{
printf("!! Pickup block 2\n");
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
mySony->panTilt(0, -25);
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("###### PickUpBlock2: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("###### PickUpBlock2: successful\n");
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
break;
case STATE_ACQUIRE_DROP_WALL:
if (myNewState)
{
printf("!! Acquire Drop wall, channel %d\n", myDropWall);
myNewState = false;
mySony->panTilt(0, -5);
myAcquire->activate();
myAcquire->setChannel(myDropWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myGripper->getGripState() != 2 ||
myGripper->getBreakBeamState() == 0)
{
printf("###### AcquireDropWall:: failed (lost cube)\n");
setState(STATE_BACKUP);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStart.mSecSince() > 35000)
{
printf("###### AcquireDropWall:: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireDropWall: successful\n");
setState(STATE_DRIVETO_DROP_WALL);
//handler();
return;
}
break;
case STATE_DRIVETO_DROP_WALL:
if (myNewState)
{
printf("!! Driveto Drop wall, channel %d\n", myDropWall);
myNewState = false;
myAcquire->deactivate();
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)
{
printf("!! Drop backup\n");
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->activate();
myNewState = false;
}
if (myStateStart.mSecSince() > 2000)
{
printf("###### Drop_Backup: done\n");
setState(STATE_ACQUIRE_LAP_WALL);
//handler();
return;
}
break;
case STATE_ACQUIRE_LAP_WALL:
if (myNewState)
{
printf("!! Acquire Lap wall, channel %d\n", myLapWall);
myNewState = false;
mySony->panTilt(0, -5);
myAcquire->activate();
myAcquire->setChannel(myLapWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStart.mSecSince() > 35000)
{
printf("###### AcquireLapWall:: failed\n");
setState(STATE_SWITCH);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireLapWall: successful\n");
setState(STATE_DRIVETO_LAP_WALL);
//handler();
return;
}
break;
case STATE_DRIVETO_LAP_WALL:
if (myNewState)
{
printf("!! Driveto Lap wall, channel %d\n", myLapWall);
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->activate();
myDriveTo->setChannel(myLapWall);
myBackup->deactivate();
}
if (myDriveTo->getState() == DriveTo::STATE_FAILED)
{
printf("###### DriveToLapWall: failed\n");
setState(STATE_SWITCH);
//handler();
return;
}
else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
{
printf("###### DriveToLapWall: succesful\n");
setState(STATE_SWITCH);
//handler();
return;
}
break;
case STATE_SWITCH:
printf("!! Switching walls around.\n");
tempColor = myDropWall;
myDropWall = myLapWall;
myLapWall = tempColor;
setState(STATE_ACQUIRE_BLOCK);
//handler();
return;
case STATE_FAILED:
printf("@@@@@ Failed to complete the task!\n");
myRobot->comInt(ArCommands::SONAR, 0);
ArUtil::sleep(50);
myRobot->comStr(ArCommands::SAY, "\52\77\37\62\42\70");
ArUtil::sleep(500);
Aria::shutdown();
myRobot->disconnect();
myRobot->stopRunning();
return;
}
}
ArActionDesired *DriveTo::fire(ArActionDesired currentDesired)
{
ArACTSBlob blob;
double xRel, yRel;
if (myState == STATE_START_LOOKING)
{
myGripper->gripClose();
myGripper->liftUp();
mySony->panTilt(0, -5);
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;
return NULL;
}
}
else
{
myLastSeen.setToNow();
}
if (myState == STATE_SUCCEEDED || myState == STATE_FAILED)
{
return NULL;
}
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() < 50)
{
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;
}
void TakeBlockToWall::handler(void)
{
switch (myState)
{
case STATE_START:
setState(STATE_ACQUIRE_BLOCK);
printf("Started state handling!\n");
handler();
return;
break;
case STATE_ACQUIRE_BLOCK:
if (myNewState)
{
myNewState = false;
mySony->panTilt(0, -10);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED)
{
printf("## AcqiureBlock: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("## AcquireBlock: successful\n");
setState(STATE_PICKUP_BLOCK);
handler();
return;
}
break;
case STATE_PICKUP_BLOCK:
if (myNewState)
{
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("## PickUpBlock: failed\n");
setState(STATE_BACKUP);
handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("## PickUpBlock: successful\n");
setState(STATE_ACQUIRE_WALL);
myGripper->liftUp();
handler();
return;
}
break;
case STATE_BACKUP:
if (myNewState)
{
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->activate();
}
if (myStateStart.mSecSince() > 2000)
{
printf("## Backup: done\n");
setState(STATE_PICKUP_BLOCK2);
handler();
return;
}
break;
case STATE_PICKUP_BLOCK2:
if (myNewState)
{
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("## PickUpBlock2: failed\n");
setState(STATE_FAILED);
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");
}
}