ArActionRatioInput.cpp

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     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 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArActionRatioInput.h"
#include "ArRobot.h"
AREXPORT ArActionRatioInput::ArActionRatioInput(const char *name) :
    ArAction(name, "RatioInputs vel and heading")
{
  myTransRatio = 0;
  myRotRatio = 0;
  myThrottleRatio = 0;
  myFullThrottleForwards = 0;
  myFullThrottleBackwards = 0;
  myRotAtFullForwards = 25;
  myRotAtFullBackwards = 25;
  myRotAtStopped = 50;

  myFullThrottleForwards = 0;
  myFullThrottleBackwards = 0;
  myRotAtFullForwards = 0;
  myRotAtFullBackwards = 0;
  myRotAtStopped = 0;
  myPrinting = false;

  myTransDeadZone = 10;
  myRotDeadZone = 5;
}

AREXPORT ArActionRatioInput::~ArActionRatioInput()
{
}

AREXPORT void ArActionRatioInput::addToConfig(ArConfig *config, 
                          const char *section)
{
  config->addParam(ArConfigArg(ArConfigArg::SEPARATOR), section, ArPriority::NORMAL);
  config->addParam(
      ArConfigArg("FullThrottleForwards", &myFullThrottleForwards,
              "The maximum forwards speed (0 means robot's TransVelMax) (mm/sec)", 0),
      section, ArPriority::NORMAL);

  config->addParam(
      ArConfigArg("FullThrottleBackwards", &myFullThrottleBackwards,
              "The maximum backwards speed (0 means 1/4 robot's TransVelMax) (mm/sec)", 0),
      section, ArPriority::NORMAL);

  config->addParam(
      ArConfigArg("RotAtFullForwards", &myRotAtFullForwards,
          "The maximum speed we turn at when going full forwards (0 means 1/2 robots RotVelMax) (deg/sec)", 0),
      section, ArPriority::NORMAL);

  config->addParam(
      ArConfigArg("RotAtFullBackwards", &myRotAtFullBackwards,
          "The maximum speed we turn at when going full backwards (0 means 1/2 robots RotVelMax) (deg/sec)", 0),
      section, ArPriority::NORMAL);

  config->addParam(
      ArConfigArg("RotAtStopped", &myRotAtStopped,
          "The maximum speed we turn at when stopped (0 means robot's RotVelMax) (deg/sec)", 0),
  section, ArPriority::NORMAL);

  config->addParam(ArConfigArg(ArConfigArg::SEPARATOR), section, ArPriority::NORMAL);
}

AREXPORT void ArActionRatioInput::setRatios(double transRatio, 
                        double rotRatio, 
                        double throttleRatio)
{
  setTransRatio(transRatio);
  setRotRatio(rotRatio);
  setThrottleRatio(throttleRatio);
}

AREXPORT void ArActionRatioInput::setTransRatio(double transRatio)
{
  if (transRatio > 100)
    myTransRatio = 100;
  else if (transRatio < -100)
    myTransRatio = -100;
  else
    myTransRatio = transRatio;
}

AREXPORT void ArActionRatioInput::setRotRatio(double rotRatio)
{
  if (rotRatio > 100)
    myRotRatio = 100;
  else if (rotRatio < -100)
    myRotRatio = -100;
  else
    myRotRatio = rotRatio;
}

AREXPORT void ArActionRatioInput::setThrottleRatio(double throttleRatio)
{
  if (throttleRatio > 100)
    myThrottleRatio = 100;
  else if (throttleRatio < 0)
    myThrottleRatio = 0;
  else
    myThrottleRatio = throttleRatio;
}

AREXPORT void ArActionRatioInput::setParameters(double fullThrottleForwards, 
                        double fullThrottleBackwards, 
                        double rotAtFullForwards,
                        double rotAtFullBackwards,
                        double rotAtStopped)
{
  myFullThrottleForwards = fullThrottleForwards;
  myFullThrottleBackwards = fullThrottleBackwards;
  myRotAtFullForwards = rotAtFullForwards;
  myRotAtFullBackwards = rotAtFullBackwards;
  myRotAtStopped = rotAtStopped;
}

AREXPORT void ArActionRatioInput::addFireCallback(int priority, 
                          ArFunctor *functor)
{
  myFireCallbacks.insert(std::pair<int, ArFunctor *>(priority, functor));
}

AREXPORT void ArActionRatioInput::remFireCallback(ArFunctor *functor)
{
  std::multimap<int, ArFunctor *>::iterator it;
  for (it = myFireCallbacks.begin(); it != myFireCallbacks.end(); it++)
  {
    if ((*it).second == functor)
      break;

  }
  if (it != myFireCallbacks.end())
  {
    myFireCallbacks.erase(it);
  }
  else 
    ArLog::log(ArLog::Normal, "ArActionRatioInput::RemFireCallback: could not remove callback");
}

AREXPORT void ArActionRatioInput::addActivateCallback(ArFunctor *functor, 
                        ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myActivateCallbacks.push_front(functor);
  else if (position == ArListPos::LAST)
    myActivateCallbacks.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArActionRatioInput::addActivateCallback: Invalid position.");
}

AREXPORT void ArActionRatioInput::remActivateCallback(ArFunctor *functor)
{
  myActivateCallbacks.remove(functor);
}

AREXPORT void ArActionRatioInput::addDeactivateCallback(ArFunctor *functor, 
                        ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myDeactivateCallbacks.push_front(functor);
  else if (position == ArListPos::LAST)
    myDeactivateCallbacks.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArActionRatioInput::addDeactivateCallback: Invalid position.");
}

AREXPORT void ArActionRatioInput::remDeactivateCallback(ArFunctor *functor)
{
  myDeactivateCallbacks.remove(functor);
}

AREXPORT void ArActionRatioInput::activate(void)
{
  std::list<ArFunctor *>::iterator it;

  if (!isActive())
  {
    myTransRatio = 0;
    myRotRatio = 0;
    myThrottleRatio = 0;
    for (it = myActivateCallbacks.begin(); 
     it != myActivateCallbacks.end(); 
     it++)
      (*it)->invoke();
  }
  ArAction::activate();
}

AREXPORT void ArActionRatioInput::deactivate(void)
{
  std::list<ArFunctor *>::iterator it;

  if (!isActive())
  {
    myTransRatio = 0;
    myRotRatio = 0;
    myThrottleRatio = 0;
    for (it = myDeactivateCallbacks.begin(); 
     it != myDeactivateCallbacks.end(); 
     it++)
      (*it)->invoke();
  }
  ArAction::deactivate();
}

AREXPORT ArActionDesired *ArActionRatioInput::fire(
    ArActionDesired currentDesired)
{
  std::multimap<int, ArFunctor *>::iterator it;
  //ArLog::log(ArLog::Normal, "Calling");
  // call the callbacks that'll set our ratios
  for (it = myFireCallbacks.begin(); it != myFireCallbacks.end(); it++)
  {
    (*it).second->invoke();
    /*ArLog::log(ArLog::Normal, "Called %s now %g %g %g\n", 
           (*it).second->getName(), 
           myTransRatio, myRotRatio, myThrottleRatio);
    */
  }
  
  myDesired.reset();

  if (myPrinting)
    printf("trans %.0f rot %.0f\n", myTransRatio, myRotRatio);

  double fullThrottleForwards, fullThrottleBackwards;
  double rotAtFullForwards, rotAtFullBackwards, rotAtStopped;

  if (myFullThrottleForwards < 1)
    fullThrottleForwards = myRobot->getTransVelMax();
  else
    fullThrottleForwards = myFullThrottleForwards;

  if (myFullThrottleBackwards < 1)
    fullThrottleBackwards = myRobot->getTransVelMax() / 4;
  else
    fullThrottleBackwards = myFullThrottleBackwards;

  if (myRotAtFullForwards < 1)
    rotAtFullForwards = myRobot->getRotVelMax() / 2;
  else
    rotAtFullForwards = myRotAtFullForwards;

  if (myRotAtFullBackwards < 1)
    rotAtFullBackwards = myRobot->getRotVelMax() / 2;
  else
    rotAtFullBackwards = myRotAtFullBackwards;

  if (myRotAtStopped < 1)
    rotAtStopped = myRobot->getRotVelMax();
  else
    rotAtStopped = myRotAtStopped;

  //printf("%g %g %g %g %g\n", fullThrottleForwards, fullThrottleBackwards, rotAtFullForwards, rotAtFullBackwards, rotAtStopped);
  // forwards
  if (myTransRatio > myTransDeadZone)
  {
    myDesired.setVel(myTransRatio/100.0 * 
             fullThrottleForwards * myThrottleRatio/100.0);
    //double totalThrottle = ArMath::fabs(myTransRatio/100.0 * 
    //myThrottleRatio/100.0);
    double speedRatio = fabs(myRobot->getVel() / fullThrottleForwards);
    if (ArMath::fabs(myRotRatio) < myRotDeadZone)
      myDesired.setRotVel(0);
    else
      myDesired.setRotVel(
          myRotRatio/100.0 * ((rotAtFullForwards - rotAtStopped) * 
                  speedRatio + rotAtStopped));
    if (myPrinting)
      printf("forwards %.0f %.0f %.0f %.0f\n", myRotRatio/100.0, rotAtFullForwards - rotAtStopped, speedRatio, rotAtStopped);

  }
  // backwards
  else if (myTransRatio < -myTransDeadZone)
  {
    myDesired.setVel(myTransRatio/100.0 * 
             fullThrottleBackwards * myThrottleRatio/100.0);
    //double totalThrottle = ArMath::fabs(myTransRatio/100.0 * 
    //myThrottleRatio/100.0);
    double speedRatio = fabs(myRobot->getVel() / fullThrottleForwards);
    if (ArMath::fabs(myRotRatio) < myRotDeadZone)
      myDesired.setRotVel(0);
    else
      myDesired.setRotVel(
        myRotRatio/100.0 * ((rotAtFullBackwards - rotAtStopped) * 
                speedRatio + rotAtStopped));
    if (myPrinting)
      printf("backwards %.0f %.0f %.0f %.0f\n", myRotRatio/100.0, rotAtFullForwards - rotAtStopped, speedRatio, rotAtStopped);
  }
  else
  {
    myDesired.setVel(0);
    if (ArMath::fabs(myRotRatio) < myRotDeadZone)
      myDesired.setRotVel(0);
    else
      myDesired.setRotVel(myRotRatio/100.0 * rotAtStopped);
  }
  
  if (myPrinting)
    printf("ratioInput %.0f %.0f\n", myDesired.getVel(), myDesired.getRotVel());


  // see if we need to up the decel
  if ((myRobot->getVel() > 0 && myTransRatio < -50) || 
      (myRobot->getVel() < 0 && myTransRatio > 50))
  {
    if (myPrinting)
      printf("Decelerating trans more\n");
    myDesired.setTransDecel(myRobot->getTransDecel() * 3, 
                ArActionDesired::MIN_STRENGTH);
  }

  // if they have the stick the opposite direction of the velocity
  // then let people crank up the deceleration
  if ((myRobot->getRotVel() > 0 && myRotRatio < -50) || 
      (myRobot->getRotVel() < 0 && myRotRatio > 50))
  {
    if (myPrinting)
      printf("Decelerating rot more\n");
    myDesired.setRotDecel(myRobot->getRotDecel() * 3,
              ArActionDesired::MIN_STRENGTH);
  }

  return &myDesired;
}

---