//-----------------------------------------------------------------------------
int main(int argc, char *argv[])
{
try
{
Acquire go;
if(!go.initialize(argc, argv))
exit(0);
go.run();
}
catch (gpstk::Exception &exc)
{ cerr << exc << endl; }
catch (std::exception &exc)
{ cerr << "Caught std::exception " << exc.what() << endl; }
catch (...)
{ cerr << "Caught unknown exception" << endl; }
}
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;
myAMPTU->panTilt(0, -40);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (have cube?)\n");
setState(STATE_PICKUP_BACKUP);
//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 ||
myStateStartTime.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;
myAMPTU->panTilt(0, -35);
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->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_PICKUP_BACKUP);
//handler();
return;
}
break;
case STATE_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### Backup: Failed, going forwards\n");
myRobot->clearDirectMotion();
setState(STATE_FORWARD);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) > BACKUP_DIST * .95 * .75)
{
printf("###### Backup: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_BLOCK2);
//handler();
return;
}
break;
case STATE_FORWARD:
if (myNewState)
{
myNewState = false;
myRobot->move(-BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### Forward: Failed\n");
myRobot->clearDirectMotion();
setState(STATE_FAILED);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### Forward: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_BLOCK2);
//handler();
return;
}
break;
case STATE_ACQUIRE_BLOCK2:
if (myNewState)
{
printf("!! Acquire block 2\n");
myNewState = false;
myAMPTU->panTilt(0, -40);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (have cube?)\n");
setState(STATE_PICKUP_BACKUP);
//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 ||
myStateStartTime.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();
myAMPTU->panTilt(0, -55);
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->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_PICKUP_BACKUP);
//handler();
return;
}
break;
case STATE_PICKUP_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95))
{
printf("###### PickUp_BackUp: done\n");
myRobot->clearDirectMotion();
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;
myAMPTU->panTilt(0, -30);
myAcquire->activate();
myAcquire->setChannel(myDropWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() != 2 ||
myGripper->getBreakBeamState() == 0)
{
printf("###### AcquireDropWall:: failed (lost cube %d %d)\n",
myGripper->getGripState(), myGripper->getBreakBeamState());
setState(STATE_BACKUP);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.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("!! DropOff Drop wall, channel %d\n", myDropWall);
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->activate();
myDropOff->setChannel(myDropWall);
myTableLimiter->deactivate();
}
if (myDropOff->getState() == DropOff::STATE_FAILED)
{
printf("###### DropOffDropWall: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myDropOff->getState() == DropOff::STATE_SUCCEEDED)
{
printf("###### DropOffDropWall: succesful\n");
setState(STATE_DROP_BACKUP);
//handler();
return;
}
break;
case STATE_DROP_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95))
{
printf("###### Drop_Backup: done\n");
myRobot->clearDirectMotion();
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;
myAMPTU->panTilt(0, -30);
myAcquire->activate();
myAcquire->setChannel(myLapWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->activate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.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);
myDropOff->deactivate();
myTableLimiter->activate();
}
if (myDriveTo->getState() == DriveTo::STATE_FAILED)
{
printf("###### DriveToLapWall: failed\n");
setState(STATE_BACKUP_LAP_WALL);
//handler();
return;
}
else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
{
printf("###### DriveToLapWall: succesful\n");
setState(STATE_BACKUP_LAP_WALL);
//handler();
return;
}
break;
case STATE_BACKUP_LAP_WALL:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### BackupLapWall: Failed, going forwards\n");
myRobot->clearDirectMotion();
setState(STATE_FORWARD_LAP_WALL);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### BackupLapWall: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_SWITCH);
//handler();
return;
}
break;
case STATE_FORWARD_LAP_WALL:
if (myNewState)
{
myNewState = false;
myRobot->move(-BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### ForwardLapWall: Failed\n");
myRobot->clearDirectMotion();
setState(STATE_FAILED);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### ForwardLapWall: Succeeded\n");
myRobot->clearDirectMotion();
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;
}
}
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;
}
}
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");
}
}
