ArActionDriveDistance.cpp

00001 /*
00002 MobileRobots Advanced Robotics Interface for Applications (ARIA)
00003 Copyright (C) 2004,2005 ActivMedia Robotics LLC
00004 Copyright (C) 2006 MobileRobots, Inc.
00005 
00006      This program is free software; you can redistribute it and/or modify
00007      it under the terms of the GNU General Public License as published by
00008      the Free Software Foundation; either version 2 of the License, or
00009      (at your option) any later version.
00010 
00011      This program is distributed in the hope that it will be useful,
00012      but WITHOUT ANY WARRANTY; without even the implied warranty of
00013      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014      GNU General Public License for more details.
00015 
00016      You should have received a copy of the GNU General Public License
00017      along with this program; if not, write to the Free Software
00018      Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019 
00020 If you wish to redistribute ARIA under different terms, contact 
00021 MobileRobots for information about a commercial version of ARIA at 
00022 robots@mobilerobots.com or 
00023 MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
00024 */
00025 
00026 #include "ArExport.h"
00027 #include "ariaOSDef.h"
00028 #include "ArActionDriveDistance.h"
00029 #include "ArRobot.h"
00030 
00031 AREXPORT ArActionDriveDistance::ArActionDriveDistance(const char *name,
00032                               double speed,
00033                               double deceleration) :
00034   ArAction(name, "Drives a given distance.")
00035 {
00036   myPrinting = false;
00037 
00038   setNextArgument(ArArg("speed", &mySpeed, 
00039             "Speed to travel to at. (mm/sec)"));
00040   setNextArgument(ArArg("deceleration", &myDeceleration, 
00041             "Speed to decelerate at. (mm/sec/sec)"));
00042   mySpeed = speed;
00043   myState = STATE_NO_DISTANCE;
00044 }
00045 
00046 AREXPORT ArActionDriveDistance::~ArActionDriveDistance()
00047 {
00048 
00049 }
00050 
00051 AREXPORT bool ArActionDriveDistance::haveAchievedDistance(void)
00052 {
00053   if (myState == STATE_ACHIEVED_DISTANCE)
00054     return true;
00055   else
00056     return false;
00057 }
00058 
00059 AREXPORT void ArActionDriveDistance::cancelDistance(void)
00060 {
00061   myState = STATE_NO_DISTANCE;
00062 }
00063 
00064 AREXPORT void ArActionDriveDistance::setDistance(
00065     double distance, bool useEncoders)
00066 {
00067   myState = STATE_GOING_DISTANCE;
00068   myDistance = distance;
00069   myUseEncoders = useEncoders;
00070   if (myUseEncoders)
00071     myLastPose = myRobot->getEncoderPose();
00072   else
00073     myLastPose = myRobot->getPose();
00074   myDistTravelled = 0;
00075   myLastVel = 0;
00076 }
00077 
00078 
00079 AREXPORT ArActionDesired *ArActionDriveDistance::fire(
00080     ArActionDesired currentDesired)
00081 {
00082   double distToGo;
00083   double vel;
00084 
00085   // if we're there we don't do anything
00086   if (myState == STATE_ACHIEVED_DISTANCE || myState == STATE_NO_DISTANCE)
00087     return NULL;
00088 
00089 
00090   if (myUseEncoders)
00091   {
00092     myDistTravelled += myRobot->getEncoderPose().findDistanceTo(myLastPose);
00093     myLastPose = myRobot->getEncoderPose();
00094   }
00095   else
00096   {
00097     myDistTravelled += myRobot->getPose().findDistanceTo(myLastPose);
00098     myLastPose = myRobot->getPose();
00099   }
00100 
00101   if (myDistance >= 0)
00102     distToGo = myDistance - myDistTravelled;
00103   else
00104     distToGo = -myDistance - myDistTravelled;
00105 
00106   if (distToGo <= 0 && ArMath::fabs(myRobot->getVel() < 5))
00107   {
00108     if (myPrinting)
00109     {
00110       ArLog::log(ArLog::Normal, 
00111          "Achieved distToGo %.0f realVel %.0f realVelDelta %.0f", 
00112          distToGo, myRobot->getVel(), myRobot->getVel() - myLastVel);
00113     }
00114     myState = STATE_ACHIEVED_DISTANCE;
00115     myDesired.setVel(0);
00116     myDesired.setRotVel(0);
00117     return &myDesired;  
00118   }
00119 
00120   myDesired.setRotVel(0);
00121   // if we're close, stop
00122   if (distToGo <= 0)
00123   {
00124     myDesired.setVel(0);
00125     vel = 0;
00126   }
00127   else
00128   {
00129     vel = sqrt(distToGo * myDeceleration * 2);
00130     if (vel > mySpeed)
00131       vel = mySpeed;
00132     if (myDistance < 0)
00133       vel *= -1;
00134     myDesired.setVel(vel);
00135   }
00136   if (myPrinting)
00137     ArLog::log(ArLog::Normal, 
00138            "distToGo %.0f cmdVel %.0f realVel %.0f realVelDelta %.0f", 
00139            distToGo, vel, myRobot->getVel(), 
00140            myRobot->getVel() - myLastVel);
00141   myLastVel = myRobot->getVel();
00142   return &myDesired;
00143 }
00144 

Generated on Fri Dec 1 10:55:11 2006 for Aria by  doxygen 1.4.7