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