Main Page | Class Hierarchy | Alphabetical List | Class List | File List | Class Members | Related Pages

ArActionLimiterBackwards.cpp

00001 /*
00002 ActivMedia Robotics Interface for Applications (ARIA)
00003 Copyright (C) 2004,2005 ActivMedia Robotics, LLC
00004 
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 ActivMedia Robotics for information about a commercial version of ARIA at 
00022 robots@activmedia.com or 
00023 ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
00024 
00025 */
00026 
00027 #include "ArExport.h"
00028 #include "ariaOSDef.h"
00029 #include "ArActionLimiterBackwards.h"
00030 #include "ArRobot.h"
00031 
00039 ArActionLimiterBackwards::ArActionLimiterBackwards(
00040         const char *name, double stopDistance, double slowDistance, 
00041         double maxBackwardsSpeed, double widthRatio) :
00042   ArAction(name,
00043            "Slows the robot down so as not to hit anything in front of it.")
00044 {
00045   setNextArgument(ArArg("stop distance", &myStopDist, 
00046                         "Distance at which to stop. (mm)"));
00047   myStopDist = stopDistance;
00048 
00049   setNextArgument(ArArg("slow distance", &mySlowDist, 
00050                         "Distance at which to slow down. (mm)"));
00051   mySlowDist = slowDistance;
00052 
00053   setNextArgument(ArArg("maximum backwards speed", &myMaxBackwardsSpeed, 
00054                          "Maximum backwards speed, scales from this to 0 at stopDistance (-mm/sec)"));
00055   myMaxBackwardsSpeed = maxBackwardsSpeed;
00056 
00057   setNextArgument(ArArg("width ratio", &myWidthRatio, 
00058                          "The ratio of robot width to how wide an area to check (ratio)"));
00059   myWidthRatio = widthRatio;
00060   
00061 }
00062 
00063 ArActionLimiterBackwards::~ArActionLimiterBackwards()
00064 {
00065 
00066 }
00067 
00068 ArActionDesired *
00069 ArActionLimiterBackwards::fire(ArActionDesired currentDesired)
00070 {
00071   double dist;
00072   double maxVel;
00073 
00074   myDesired.reset();
00075   dist = myRobot->checkRangeDevicesCurrentBox(-myRobot->getRobotLength()/2,
00076                                               -(myRobot->getRobotWidth()/2.0 * 
00077                                                 myWidthRatio),
00078                                mySlowDist + (-myRobot->getRobotLength()),
00079                                               (myRobot->getRobotWidth()/2.0 * 
00080                                                myWidthRatio));
00081   dist -= myRobot->getRobotRadius();
00082   if (dist < -myStopDist)
00083   {
00084     //printf("backwards stop\n");
00085     myDesired.setMaxNegVel(0);
00086     return &myDesired;
00087   }
00088   if (dist > -mySlowDist)
00089   {
00090     //printf("backwards nothing\n");
00091     myDesired.setMaxNegVel(-ArMath::fabs(myMaxBackwardsSpeed));
00092     return &myDesired;
00093   }
00094       
00095                         
00096   maxVel = -ArMath::fabs(myMaxBackwardsSpeed) * ((-dist - myStopDist) / (mySlowDist - myStopDist));
00097   //printf("Neg Max vel %f (stopdist %.1f slowdist %.1f slowspeed %.1f\n", maxVel,       myStopDist, mySlowDist, myMaxBackwardsSpeed);
00098   myDesired.setMaxNegVel(maxVel);
00099   return &myDesired;
00100   
00101 }

Generated on Wed Oct 19 12:56:34 2005 for Aria by  doxygen 1.4.0