ArActionDeceleratingLimiter.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 "ArActionDeceleratingLimiter.h"
#include "ArRobot.h"
#include "ArCommands.h"
#include "ariaInternal.h"
#include "ArRobotConfigPacketReader.h"

ArActionDeceleratingLimiter::ArActionDeceleratingLimiter(
        const char *name, 
        bool forwards) :
  ArAction(name,
           "Slows the robot down and cranks up deceleration so as not to hit anything in front of it.")
{
  myForwards = forwards;
  setParameters();

  myLastStopped = false;
}

ArActionDeceleratingLimiter::~ArActionDeceleratingLimiter()
{

}

void ArActionDeceleratingLimiter::setParameters(
        double clearance,
        double sideClearanceAtSlowSpeed,
        double paddingAtSlowSpeed,
        double slowSpeed,
        double sideClearanceAtFastSpeed,
        double paddingAtFastSpeed,
        double fastSpeed,
        double preferredDecel,
        bool useEStop,
        double maxEmergencyDecel)
{
  myClearance = clearance;
  mySideClearanceAtSlowSpeed = sideClearanceAtSlowSpeed;
  myPaddingAtSlowSpeed = paddingAtSlowSpeed;
  mySlowSpeed = slowSpeed;
  mySideClearanceAtFastSpeed = sideClearanceAtFastSpeed;
  myPaddingAtFastSpeed = paddingAtFastSpeed;
  myFastSpeed = fastSpeed;
  myPreferredDecel = preferredDecel;
  myUseEStop = useEStop;
  myMaxEmergencyDecel = maxEmergencyDecel;
}

void ArActionDeceleratingLimiter::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 += "Clearance";
  config->addParam(
          ArConfigArg(name.c_str(), &myClearance, 
                      "Don't get closer than this to something in front or back. (mm)"), 
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "SlowSpeed";
  config->addParam(
          ArConfigArg(name.c_str(),
                      &mySlowSpeed,
                      "Consider this speed slow (mm/sec)"),
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "SideClearanceAtSlowSpeed";
  config->addParam(
          ArConfigArg(name.c_str(),
                      &mySideClearanceAtSlowSpeed, 
                      "Don't get closer than this to something on the side if we're going at slow speed or below. (mm)"),
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "PaddingAtSlowSpeed";
  config->addParam(
          ArConfigArg(name.c_str(), &myPaddingAtSlowSpeed, 
                      "Try to stop this far away from clearance at slow speed or below. (mm)"), 
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "FastSpeed";
  config->addParam(
          ArConfigArg(name.c_str(),
                      &myFastSpeed,
                      "Consider this speed fast (mm/sec)"),
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "SideClearanceAtFastSpeed";
  config->addParam(
          ArConfigArg(name.c_str(),
                      &mySideClearanceAtFastSpeed, 
                      "Don't get closer than this to something on the side if we're going at fast speed or above. (mm)"),
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "PaddingAtFastSpeed";
  config->addParam(
          ArConfigArg(name.c_str(), &myPaddingAtFastSpeed, 
                      "Try to stop this far away from clearance at fast speed or below. (mm)"), 
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "PreferredDecel";
  config->addParam(
          ArConfigArg(name.c_str(),
                      &myPreferredDecel,
                      "The maximum decel we'll use until something might infringe on clearance and sideClearanceAtSlowSpeed (mm/sec/sec"),
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "MaxEmergencyDecel";
  config->addParam(
          ArConfigArg(name.c_str(),
                      &myMaxEmergencyDecel,
                      "The maximum decel we'll ever use, 0 means use the robot's maximum (mm/sec/sec"),
          section, ArPriority::NORMAL);

  name = strPrefix;
  name += "UseEStop";
  config->addParam(
          ArConfigArg(name.c_str(),
                      &myUseEStop,
                      "Whether to use an EStop to stop if something will intrude on our clearance"),
          section, ArPriority::NORMAL);
}

ArActionDesired *
ArActionDeceleratingLimiter::fire(ArActionDesired currentDesired)
{
  double dist;
  double distInner;
  bool printing = false;
  double absVel;
  
  myDesired.reset();
  // see if we're going the right direction for this to work
  if (myRobot->getVel() < -100 && myForwards)
    return NULL;
  else if (myRobot->getVel() > 100 && !myForwards)
    return NULL;
  absVel = ArMath::fabs(myRobot->getVel());

  double sideClearance;
  double padding;
  
  // see if we're going slow
  if (ArMath::fabs(myRobot->getVel()) <= mySlowSpeed)
  {
    sideClearance = mySideClearanceAtSlowSpeed;
    padding = myPaddingAtSlowSpeed;
  }
  // or if we're going fast
  else if (ArMath::fabs(myRobot->getVel()) >= myFastSpeed)
  {
    sideClearance = mySideClearanceAtFastSpeed;
    padding = myPaddingAtFastSpeed;
  }
  // or if we have to interpolate
  else
  {
    sideClearance = (((mySideClearanceAtFastSpeed - 
                       mySideClearanceAtSlowSpeed) * 
                      ((ArMath::fabs(myRobot->getVel()) - mySlowSpeed) / 
                       (myFastSpeed - mySlowSpeed))) + 
                     mySideClearanceAtSlowSpeed);
    padding = (((myPaddingAtFastSpeed - 
                 myPaddingAtSlowSpeed) * 
                ((ArMath::fabs(myRobot->getVel()) - mySlowSpeed) / 
                 (myFastSpeed - mySlowSpeed))) + 
               myPaddingAtSlowSpeed);
  }


  if (printing)
    printf("%d side %.0f padding %.0f\n", myForwards, sideClearance, padding);
                          
  if (myForwards)
    dist = myRobot->checkRangeDevicesCurrentBox(
            0,
            -(myRobot->getRobotWidth()/2.0 + sideClearance),
            myRobot->getRobotLength()/2.0 + myClearance + padding + 8000,
            (myRobot->getRobotWidth()/2.0 + sideClearance));
  else
    dist = myRobot->checkRangeDevicesCurrentBox(
            0,
            -(myRobot->getRobotWidth()/2.0 + sideClearance),
            -(myRobot->getRobotLength()/2.0 + myClearance + padding + 8000),
            (myRobot->getRobotWidth()/2.0 + sideClearance));

  if (myForwards)
    distInner = myRobot->checkRangeDevicesCurrentBox(
            0,
            -(myRobot->getRobotWidth()/2.0 + mySideClearanceAtSlowSpeed),
            myRobot->getRobotLength()/2.0 + myClearance + 8000,
            (myRobot->getRobotWidth()/2.0 + mySideClearanceAtSlowSpeed));
  else
    distInner = myRobot->checkRangeDevicesCurrentBox(
            0,
            -(myRobot->getRobotWidth()/2.0 + mySideClearanceAtSlowSpeed),
            -(myRobot->getRobotLength()/2.0 + myClearance + 8000),
            (myRobot->getRobotWidth()/2.0 + mySideClearanceAtSlowSpeed));
    
  // subtract off our clearance and padding to see how far we have to stop
  dist -= myRobot->getRobotLength() / 2.0;
  dist -= myClearance;
  dist -= padding;

  // this is what we estop for, so don't subtract our padding from this
  distInner -= myRobot->getRobotLength() / 2.0;
  distInner -= myClearance;

  if (printing)
    printf("%d dist %.0f\n", myForwards, dist);
  // see if we need to throw an estop
  if (distInner < 1 && ((myRobot->getVel() > 5 && myForwards) ||
                   (myRobot->getVel() < -5 && !myForwards)))
  {
    if (printing && !myLastStopped)
      printf("%d Stopping\n", myForwards);
    if (ArMath::fabs(myRobot->getVel()) > 100)
    {
      if (myUseEStop)
      {
        ArLog::log(ArLog::Normal, "ArActionDeceleratingLimiter: estopping");
        myRobot->comInt(ArCommands::ESTOP, 1);
      }
      else
      {
        ArLog::log(ArLog::Verbose, "ArActionDeceleratingLimiter: maximum deceleration");
      }
      // if we have a maximum emergency decel, use that
      if (fabs(myMaxEmergencyDecel) > 1)
      {
        if (printing)
          printf("Max emergency decel %.0f\n", 
                 myMaxEmergencyDecel);
        myDesired.setTransDecel(myMaxEmergencyDecel);
      }
      //  if we don't use the robot's top decel
      else if (myRobot->getOrigRobotConfig() != NULL && 
          myRobot->getOrigRobotConfig()->hasPacketArrived())
      {
        if (printing)
          printf("Robots max decel %d\n", 
                 myRobot->getOrigRobotConfig()->getTransAccelTop());
        myDesired.setTransDecel(
                myRobot->getOrigRobotConfig()->getTransAccelTop());
      }
      // if we don't have that either use our preferred decel
      else
      {
        if (printing)
          printf("Prefered decel %g\n", myPreferredDecel);
        myDesired.setTransDecel(myPreferredDecel);
      }
    }
    myLastStopped = true;
    if (myForwards)
      myDesired.setMaxVel(0);
    else
      myDesired.setMaxNegVel(0);
    myDesired.setVel(0);
    return &myDesired;
  }
  // if our distance is greater than how far it'd take us to stop
  //printf("%.0f %.0f %.0f\n", dist, absVel, absVel * absVel / 2.0 / myRobot->getTransDecel());
  if (dist > absVel * absVel / 2.0 / myRobot->getTransDecel())
  {
    if (printing)
      printf("%d Nothing\n", myForwards);
    return NULL;
  }

  if (printing && myLastStopped)
    printf("%d moving\n", myForwards);
  myLastStopped = false;
  //printf("%f ", dist);
  //maxVel = (dist - clearance);
  double deceleration = - absVel * absVel / dist / 2.0;
  double decelerationInner = - absVel * absVel / distInner / 2.0;
  // make sure the robot or other actions aren't already decelerating
  // more than we want to

  //printf("%.0f %.0f %.0f %.0f\n", deceleration, myRobot->getTransDecel(),      currentDesired.getTransDecelStrength(), currentDesired.getTransDecel());
  if (fabs(deceleration) > fabs(myRobot->getTransDecel()) &&
      (currentDesired.getTransDecelStrength() < ArActionDesired::MIN_STRENGTH 
       || fabs(deceleration) > fabs(currentDesired.getTransDecel())))
  {
    // if our deceleration is faster than we want to decel see if we
    // actually will have to decel that fast or not
    if (fabs(myMaxEmergencyDecel) > 1 && 
        fabs(decelerationInner) > myMaxEmergencyDecel)
      myDesired.setTransDecel(myMaxEmergencyDecel);
    else if (fabs(decelerationInner) > myPreferredDecel)
      myDesired.setTransDecel(fabs(decelerationInner));
    else if (fabs(myMaxEmergencyDecel) > 1 && 
             fabs(deceleration) > myMaxEmergencyDecel)
      myDesired.setTransDecel(myMaxEmergencyDecel);
    else if (fabs(deceleration) > myPreferredDecel)
      myDesired.setTransDecel(myPreferredDecel);
    else 
      myDesired.setTransDecel(fabs(deceleration));
    if (printing)
      printf("Set deceleration to %g\n", myDesired.getTransDecel());
  }
  else
    deceleration = myRobot->getTransDecel();

  //double maxVel = absVel - deceleration  / 10.0;

  if (printing)
    printf("%d accel %.0f\n", myForwards, deceleration);
  //printf("Max vel %f (stopdist %.1f slowdist %.1f slowspeed %.1f\n", maxVel,   myStopDist, mySlowDist, mySlowSpeed);

  if (myForwards)
    myDesired.setMaxVel(0);
  else
    myDesired.setMaxNegVel(0);
  return &myDesired;
}

---