ArActionMovementParameters.cpp

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArActionMovementParameters.h"
#include "ArConfig.h"

ArActionMovementParameters::ArActionMovementParameters(
        const char *name) : 
  ArAction(name, "Sets all the max vel and accels/decels")
{
  setParameters();
}

ArActionMovementParameters::~ArActionMovementParameters()
{

}

void ArActionMovementParameters::setParameters(double maxVel, 
                                                   double maxNegVel,
                                                   double transAccel,
                                                   double transDecel,
                                                   double rotVelMax, 
                                                   double rotAccel,
                                                   double rotDecel)
{
  myMaxVel = maxVel;
  myMaxNegVel = maxNegVel;
  myTransAccel = transAccel;
  myTransDecel = transDecel;
  myMaxRotVel = rotVelMax;
  myRotAccel = rotAccel;
  myRotDecel = rotDecel;
}

void ArActionMovementParameters::addToConfig(ArConfig *config, 
                                                      const char *section, 
                                                      const char *prefix)
{
  std::string strPrefix;
  std::string name;
  if (prefix == NULL || prefix[0] == '\0')
    strPrefix = "";
  else
    strPrefix = prefix;

  name = strPrefix;
  name += "TransVelMax";
  config->addParam(
          ArConfigArg(name.c_str(), &myMaxVel, 
                      "Maximum forward translational velocity (0 means use default)", 
                      0),
          section, ArPriority::DETAILED);


  name = strPrefix;
  name += "TransNegVelMax";
  config->addParam(
          ArConfigArg(name.c_str(), &myMaxNegVel, 
                      "Maximum backwards translational velocity (0 means use default)", 
                      0),
          section, ArPriority::DETAILED);

  name = strPrefix;
  name += "TransAccel";
  config->addParam(
          ArConfigArg(name.c_str(), &myTransAccel, 
                      "Translational acceleration (0 means use default)", 0),
          section, ArPriority::DETAILED);

  name = strPrefix;
  name += "TransDecel";
  config->addParam(
          ArConfigArg(name.c_str(), &myTransDecel, 
                      "Translational deceleration (0 means use default)", 0),
          section, ArPriority::DETAILED);

  name = strPrefix;
  name += "RotVelMax";
  config->addParam(
          ArConfigArg(name.c_str(), &myMaxRotVel, 
                      "Maximum rotational velocity (0 means use default)", 
                      0),
          section, ArPriority::DETAILED);

  name = strPrefix;
  name += "RotAccel";
  config->addParam(
          ArConfigArg(name.c_str(), &myRotAccel, 
                      "Rotational acceleration (0 means use default)", 0),
          section, ArPriority::DETAILED);

  name = strPrefix;
  name += "RotDecel";
  config->addParam(
          ArConfigArg(name.c_str(), &myRotDecel, 
                      "Rotational deceleration (0 means use default)", 0),
          section, ArPriority::DETAILED);



}

ArActionDesired *ArActionMovementParameters::fire(
        ArActionDesired currentDesired)
{
  myDesired.reset();

  if (fabs(myMaxVel) >= 1)
  {
    // if we're set to a larger value than we have use full strength
    // (to try to go slower)
    if (currentDesired.getMaxVelStrength() > ArActionDesired::MIN_STRENGTH 
        && fabs(myMaxVel) < fabs(currentDesired.getMaxVel()))
      myDesired.setMaxVel(myMaxVel);
    // otherwise use as little as possible so other things can do more
    // if they need to
    else
      myDesired.setMaxVel(myMaxVel, ArActionDesired::MIN_STRENGTH);
  }

  if (fabs(myMaxNegVel) >= 1)
  {
    // if we're set to a larger value than we have use full strength
    // (to try to go slower)
    if (currentDesired.getMaxNegVelStrength() > ArActionDesired::MIN_STRENGTH 
        && fabs(myMaxNegVel) < fabs(currentDesired.getMaxNegVel()))
      myDesired.setMaxNegVel(myMaxNegVel);
    // otherwise use as little as possible so other things can do more
    // if they need to
    else
      myDesired.setMaxNegVel(myMaxNegVel, ArActionDesired::MIN_STRENGTH);
  }

  if (fabs(myTransAccel) >= 1)
  {    
    
    // if we're set to a larger value than we have use full strength
    // (to try to go slower)
    if (currentDesired.getTransAccelStrength() > ArActionDesired::MIN_STRENGTH 
        && fabs(myTransAccel) < fabs(currentDesired.getTransAccel()))
      myDesired.setTransAccel(myTransAccel);
    // otherwise use as little as possible so other things can do more
    // if they need to
    else
      myDesired.setTransAccel(myTransAccel, ArActionDesired::MIN_STRENGTH);
  }

  if (fabs(myTransDecel) >= 1)
  {
    // if we're set to a larger value than we have use full strength
    // (to try to go slower)
    if (currentDesired.getTransDecelStrength() > ArActionDesired::MIN_STRENGTH 
        && fabs(myTransDecel) < fabs(currentDesired.getTransDecel()))
      myDesired.setTransDecel(myTransDecel);
    // otherwise use as little as possible so other things can do more
    // if they need to
    else
      myDesired.setTransDecel(myTransDecel, ArActionDesired::MIN_STRENGTH);
  }

  if (fabs(myMaxRotVel) >= 1)
  {
    // if we're set to a larger value than we have use full strength
    // (to try to go slower)
    if (currentDesired.getMaxRotVelStrength() > ArActionDesired::MIN_STRENGTH 
        && fabs(myMaxRotVel) < fabs(currentDesired.getMaxRotVel()))
      myDesired.setMaxRotVel(myMaxRotVel);

    // otherwise use as little as possible so other things can do more
    // if they need to
    else
      myDesired.setMaxRotVel(myMaxRotVel, ArActionDesired::MIN_STRENGTH);
  }


  if (fabs(myRotAccel) >= 1)
  {
    // if we're set to a larger value than we have use full strength
    // (to try to go slower)
    if (currentDesired.getRotAccelStrength() > ArActionDesired::MIN_STRENGTH 
        && fabs(myRotAccel) < fabs(currentDesired.getRotAccel()))
      myDesired.setRotAccel(myRotAccel);
    // otherwise use as little as possible so other things can do more
    // if they need to
    else
      myDesired.setRotAccel(myRotAccel, ArActionDesired::MIN_STRENGTH);
  }

  if (fabs(myRotDecel) >= 1)
  {
    // if we're set to a larger value than we have use full strength
    // (to try to go slower)
    if (currentDesired.getRotDecelStrength() > ArActionDesired::MIN_STRENGTH 
        && fabs(myRotDecel) < fabs(currentDesired.getRotDecel()))
      myDesired.setRotAccel(myRotDecel);
    // otherwise use as little as possible so other things can do more
    // if they need to
    else
      myDesired.setRotDecel(myRotDecel, ArActionDesired::MIN_STRENGTH);
  }
  
  return &myDesired;
}

---