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