AREXPORT void ArForbiddenRangeDevice::processReadings(void) { ArPose intersection; std::list<ArLineSegment *>::iterator it; lockDevice(); myDataMutex.lock(); myCurrentBuffer.beginRedoBuffer(); if (!myIsEnabled) { myCurrentBuffer.endRedoBuffer(); myDataMutex.unlock(); unlockDevice(); return; } ArLineSegment *segment; ArPose start; double startX; double startY; ArPose end; double angle; double length; double gone; double sin; double cos; double atX; double atY; double robotX = myRobot->getX(); double robotY = myRobot->getY(); double max = (double) myMaxRange; double maxSquared = (double) myMaxRange * (double) myMaxRange; ArTime startingTime; //startingTime.setToNow(); // now see if the end points of the segments are too close to us for (it = mySegments.begin(); it != mySegments.end(); it++) { segment = (*it); // if either end point or some perpindicular point is close to us // add the line's data if (ArMath::squaredDistanceBetween( segment->getX1(), segment->getY1(), myRobot->getX(), myRobot->getY()) < maxSquared || ArMath::squaredDistanceBetween( segment->getX2(), segment->getY2(), myRobot->getX(), myRobot->getY()) < maxSquared || segment->getPerpDist(myRobot->getPose()) < max) { start.setPose(segment->getX1(), segment->getY1()); end.setPose(segment->getX2(), segment->getY2()); angle = start.findAngleTo(end); cos = ArMath::cos(angle); sin = ArMath::sin(angle); startX = start.getX(); startY = start.getY(); length = start.findDistanceTo(end); // first put in the start point if we should if (ArMath::squaredDistanceBetween( startX, startY, robotX, robotY) < maxSquared) myCurrentBuffer.redoReading(start.getX(), start.getY()); // now walk the length of the line and see if we should put the points in for (gone = 0; gone < length; gone += myDistanceIncrement) { atX = startX + gone * cos; atY = startY + gone * sin; if (ArMath::squaredDistanceBetween( atX, atY, robotX, robotY) < maxSquared) myCurrentBuffer.redoReading(atX, atY); } // now check the end point if (end.squaredFindDistanceTo(myRobot->getPose()) < maxSquared) myCurrentBuffer.redoReading(end.getX(), end.getY()); } } myDataMutex.unlock(); // and we're done myCurrentBuffer.endRedoBuffer(); unlockDevice(); //printf("%d\n", startingTime.mSecSince()); }
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; }
int main(int argc, char **argv) { std::string str; int ret; int dist; ArTime start; ArPose startPose; bool vel2 = false; // connection to the robot ArSerialConnection con; // the robot ArRobot robot; // the connection handler from above ConnHandler ch(&robot); // init area with a dedicated signal handling thread Aria::init(Aria::SIGHANDLE_THREAD); if (argc != 2 || (dist = atoi(argv[1])) == 0) { printf("Usage: %s <distInMM>\n", argv[0]); exit(0); } if (dist < 1000) { printf("You must go at least a meter\n"); exit(0); } // open the connection with the defaults, exit if failed if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } // set the robots connection robot.setDeviceConnection(&con); // try to connect, if we fail, the connection handler should bail if (!robot.blockingConnect()) { // this should have been taken care of by the connection handler // but just in case printf( "asyncConnect failed because robot is not running in its own thread.\n"); Aria::shutdown(); return 1; } // run the robot in its own thread, so it gets and processes packets and such robot.runAsync(false); // just a big long set of printfs, direct motion commands and sleeps, // it should be self-explanatory robot.lock(); /* robot.setAbsoluteMaxTransVel(2000); robot.setTransVelMax(2000); robot.setTransAccel(1000); robot.setTransDecel(1000); robot.comInt(82, 30); // rotkp robot.comInt(83, 200); // rotkv robot.comInt(84, 0); // rotki robot.comInt(85, 30); // transkp robot.comInt(86, 450); // transkv robot.comInt(87, 4); // transki */ printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist); if (vel2) robot.setVel2(2200, 2200); else robot.setVel(2200); robot.unlock(); start.setToNow(); startPose = robot.getPose(); while (1) { robot.lock(); printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f", robot.getVel(), robot.getX(), robot.getY(), startPose.findDistanceTo(robot.getPose()), robot.getTh()); if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000) { printf("\nFinished distance\n"); robot.setVel(0); robot.unlock(); break; } if (start.mSecSince() > 10000) { printf("\nDistance timed out\n"); robot.setVel(0); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } if (vel2) robot.setVel2(0, 0); else robot.setVel(0); start.setToNow(); while (1) { robot.lock(); if (vel2) robot.setVel2(0, 0); else robot.setVel(0); if (fabs(robot.getVel()) < 20) { printf("Stopped\n"); robot.unlock(); break; } if (start.mSecSince() > 2000) { printf("\nStop timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } robot.lock(); robot.disconnect(); robot.unlock(); // shutdown and ge tout Aria::shutdown(); return 0; }
ArActionDesired *DriveTo::fire(ArActionDesired currentDesired) { ArACTSBlob blob; double xRel, yRel; ArPose pose; double dist; if (myState == STATE_SUCCEEDED || myState == STATE_FAILED) { myDesired.setVel(0); myDesired.setDeltaHeading(0); return &myDesired; } if (myState == STATE_START_LOOKING) { myState = STATE_LOOKING; myLastSeen.setToNow(); myLastPose = myRobot->getPose(); myLastMoved.setToNow(); } pose = myRobot->getPose(); dist = myLastPose.findDistanceTo(pose); if (dist < 5 && myLastMoved.mSecSince() > 1500) { printf("DriveTo: Failed, no movement in the last 1500 msec.\n"); myState = STATE_FAILED; myDesired.setVel(0); myDesired.setDeltaHeading(0); return &myDesired; } else if (dist > 5) { myLastMoved.setToNow(); myLastPose = myRobot->getPose(); } if (myActs->getNumBlobs(myChannel) == 0 || !myActs->getBlob(myChannel, 1, &blob)) { if (myLastSeen.mSecSince() > 1000) { printf("DriveTo: Lost the blob, failed.\n"); myState = STATE_FAILED; myDesired.setVel(0); myDesired.setDeltaHeading(0); return &myDesired; } } else { myLastSeen.setToNow(); } 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\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() < 125) { 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 * 20); if (-xRel > 1.0) myDesired.setDeltaHeading(20); else if (-xRel < -1.0) myDesired.setDeltaHeading(-20); else myDesired.setDeltaHeading(-xRel * 20); } myDesired.setVel(150); 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; } }
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 SamIter(void) { Bottle *goal = GoalIn.read(false); //puts("running reader"); //while (GoalIn.getPendingReads() > 0) // goal = GoalIn.read(false); if(goal!=NULL) // check theres data { std::cout << "size of bottle is " << goal->size() << " data " << goal->toString().c_str() << std::endl; if(goal->size()>0) { //iGoalPostion =GetGoalFromString(goal->get(1).asString().c_str()); iGoalPostion=atoi(goal->get(1).asString().c_str()); SetGoalPos(iGoalPostion); bReached=false; std::cout<< "got a goal, location " << iGoalPostion <<std::endl; iStuckCount=0; loopCnt=0; robot->enableMotors(); robot->enableSonar(); } } //// send back a bottle with current voltage value //Bottle& b2 = PwrOut.prepare(); // prepare the bottle/port //b2.clear(); //b2.addDouble(robot->getRealBatteryVoltage()); // indicates robot voltage avg //PwrOut.writeStrict(); Bottle *input = NULL; //puts("running reader"); while (StarIn.getPendingReads() > 0) input = StarIn.read(false); if(input!=NULL) // check theres data { dStarId = input->get(0).asDouble(); //std::cout << "star id " << starID << std::endl; dX = input->get(1).asDouble(); dY = input->get(2).asDouble(); dTh = input->get(3).asDouble(); if(firstStarReading==false) { CurrentPose.setX(dX); CurrentPose.setY(dY); CurrentPose.setTh(dTh); OldPose = CurrentPose; firstStarReading=true; } iStarCntr++; if((OldPose.findDistanceTo(CurrentPose)<1.5 || iStarCntr>3) && dStarId!=0) { CurrentPose.setX(dX); CurrentPose.setY(dY); CurrentPose.setTh(dTh); OldPose = CurrentPose; iStarCntr=0; } //puts("got a msg"); //puts(input->toString()); //std::cout << " robot->getRealBatteryVoltageNow() " << " " << robot->getRealBatteryVoltageNow()<<std::endl; } if(bReached==false) { loopCnt++; double ang = atan2(GoalPose.getY() - CurrentPose.getY(), GoalPose.getX() - CurrentPose.getX()); double dist = GoalPose.findDistanceTo(CurrentPose); //check if the robot is stuck while navigating if(dist>0) { //first distance reading if(iStuckCount==0) OldDist=dist; DiffDist=OldDist-dist; if(loopCnt%5==0 && abs(DiffDist)<0.1) { iStuckCount++; std::cout << "diff dist, dist, stuck count " << DiffDist << " " << dist << " " << iStuckCount <<std::endl; } else OldDist=dist; //recover motion if(iStuckCount>6) { robot->lock(); robot->setVel(-100);//move back yarp::os::Time::delay(5); robot->unlock(); //robot->setVel(0);//move back iStuckCount=0; } } //calculate_distance(dX, dY, GoalPose.getX(), GoalPose.getY());//ar.findDistanceTo(ArPose(-0.75,3.0,4.5)); ang-=dTh; while(ang>3.14159265){ang-=6.28318531;} // normalise PI ie if over 180, then minus 360 while(ang<-3.14159265){ang+=6.28318531;} ang = Rad2Deg(ang); //std::cout << "required angle, dist " << ang << " " << dist <<std::endl; //if the robot has to turn more then turn threshold for obst avoidance is less and speed is 0 if(abs(ang)>55) { myTurnThreshold = 250; myStopDistance = 200; speed=0; } else { speed=220; myTurnThreshold = 550; myStopDistance = 450; } bool obst= ObstacleAvoidance(); if(dist>=0.7 && obst==false)//&& abs(ang)>10 { robot->lock(); std::cout <<"navigating to goal " << iGoalPostion<<std::endl; /* if(abs(ang)<45) speed=200; else speed=0;*/ robot->setDeltaHeading(ang/2.0); robot->setVel(speed); robot->unlock(); } else if(dist<0.7) { robot->lock(); double rotFinal = GoalPose.getTh(); rotFinal-=dTh; std::cout <<"goal rot " << rotFinal <<std::endl; while(rotFinal>3.14159265){rotFinal-=6.28318531;} // normalise PI ie if over 180, then minus 360 while(rotFinal<-3.14159265){rotFinal+=6.28318531;} rotFinal = Rad2Deg(rotFinal); robot->setDeltaHeading(rotFinal); robot->setVel(0); std::cout <<"final rot " << rotFinal <<std::endl; //robot->setDeltaHeading(0); ang=0; speed=0; puts("reached "); bReached=true; robot->unlock(); // send back a bottle with goal position reached Bottle& b2 = GoalIn.prepare(); // prepare the bottle/port b2.clear(); b2.addInt(iGoalPostion); // indicates robot reached goal location GoalIn.writeStrict(); //wait for some time for rotation to complete yarp::os::Time::delay(5); robot->disableMotors(); robot->disableSonar(); } /*myfile.open ("motion.txt"); if (myfile.is_open()) { myfile << ang << "|" << speed << "\n"; myfile.close(); }*/ } //else{puts("didn't get a msg");} }
void Joydrive::drive(void) { int trans, rot; ArPose pose; ArPose rpose; ArTransform transform; ArRangeDevice *dev; ArSensorReading *son; if (!myRobot->isConnected()) { printf("Lost connection to the robot, exiting\n"); exit(0); } printf("\rx %6.1f y %6.1f th %6.1f", myRobot->getX(), myRobot->getY(), myRobot->getTh()); fflush(stdout); if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1)) { if (ArMath::fabs(myRobot->getVel()) < 10.0) myRobot->comInt(ArCommands::ENABLE, 1); myJoyHandler.getAdjusted(&rot, &trans); myRobot->setVel(trans); myRobot->setRotVel(-rot); } else { myRobot->setVel(0); myRobot->setRotVel(0); } if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) && time(NULL) - myLastPress > 1) { myLastPress = time(NULL); printf("\n"); switch (myTest) { case 1: printf("Moving back to the origin.\n"); pose.setPose(0, 0, 0); myRobot->moveTo(pose); break; case 2: printf("Moving over a meter.\n"); pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0); myRobot->moveTo(pose); break; case 3: printf("Doing a transform test....\n"); printf("\nOrigin should be transformed to the robots coords.\n"); transform = myRobot->getToGlobalTransform(); pose.setPose(0, 0, 0); pose = transform.doTransform(pose); rpose = myRobot->getPose(); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (pose.findDistanceTo(rpose) < .1) printf("Success\n"); else printf("#### FAILURE\n"); printf("\nRobot coords should be transformed to the origin.\n"); transform = myRobot->getToLocalTransform(); pose = myRobot->getPose(); pose = transform.doTransform(pose); rpose.setPose(0, 0, 0); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (pose.findDistanceTo(rpose) < .1) printf("Success\n"); else printf("#### FAILURE\n"); break; case 4: printf("Doing a tranform test...\n"); printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n"); transform = myRobot->getToGlobalTransform(); pose.setPose(-1000, 0, 0); pose = transform.doTransform(pose); rpose = myRobot->getPose(); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1) printf("Probable Success\n"); else printf("#### FAILURE\n"); break; case 5: printf("Doing a transform test on range devices..\n"); printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n"); dev = myRobot->findRangeDevice("sonar"); if (dev == NULL) { printf("No sonar on the robot, can't do the test.\n"); break; } printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0)); printf("Sonar 0 reading is at "); son = myRobot->getSonarReading(0); if (son != NULL) { pose = son->getPose(); pose.log(); } pose = myRobot->getPose(); pose.setX(pose.getX() + 4000); pose.setY(pose.getY() + 4000); myRobot->moveTo(pose); printf("Moved robot.\n"); printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0)); printf("Sonar 0 reading is at "); son = myRobot->getSonarReading(0); if (son != NULL) { pose = son->getPose(); pose.log(); } break; case 6: printf("Robot position now is:\n"); pose = myRobot->getPose(); pose.log(); printf("Disconnecting from the robot, then reconnecting.\n"); myRobot->disconnect(); myRobot->blockingConnect(); printf("Robot position now is:\n"); pose = myRobot->getPose(); pose.log(); break; default: printf("No test for second button.\n"); break; } } }