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