int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 225, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 225, 1100, 400);
ArActionTableSensorLimiter tableLimiter;
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArPTZ *ptz;
ptz = new ArVCC4(&robot, true);
ArGripper gripper(&robot);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, ptz);
DropOff dropOff(&acts, &gripper, ptz);
PickUp pickUp(&acts, &gripper, ptz);
TakeBlockToWall takeBlock(&robot, &gripper, ptz, &acq, &driveTo, &pickUp,
&dropOff, &tableLimiter);
if (!acts.openPort(&robot))
{
printf("Could not connect to acts, exiting\n");
exit(0);
}
Aria::init();
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
ptz->init();
ArUtil::sleep(8000);
printf("### 2222\n");
ptz->panTilt(0, -40);
printf("### whee\n");
ArUtil::sleep(8000);
robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&tableLimiter, 100);
robot.addAction(&limiter, 99);
robot.addAction(&limiterFar, 98);
robot.addAction(&backwardsLimiter, 97);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&dropOff, 74);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
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;
}
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;
}
}
