ArActionDesired.h

/*
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

*/

#ifndef ARACTIONDESIRED_H
#define ARACTIONDESIRED_H

#include "ariaUtil.h"

class ArActionDesiredChannel
{
public:
  static const double NO_STRENGTH;
  static const double MIN_STRENGTH;
  static const double MAX_STRENGTH;
  
  ArActionDesiredChannel() { reset(); }
  ~ArActionDesiredChannel() {}
  void setDesired(double desired, double desiredStrength) 
    {
      myDesired = desired;
      myStrength = desiredStrength; 
      if (myStrength > MAX_STRENGTH)
        myStrength = MAX_STRENGTH;
      if (myStrength < MIN_STRENGTH)
        myStrength = NO_STRENGTH;
    }
  double getDesired(void)  { return myDesired; }
  double getStrength(void)  { return myStrength; }
  void reset(void) { myDesired = 0; myStrength = NO_STRENGTH;}
  void merge(ArActionDesiredChannel *desiredChannel)
    {
      double otherStrength = desiredChannel->getStrength();
      double oldStrength = myStrength;
      if (myStrength + otherStrength > MAX_STRENGTH)
        otherStrength = MAX_STRENGTH - myStrength;
      myStrength = myStrength + otherStrength;
      if (myStrength >= MIN_STRENGTH)
        myDesired = (((oldStrength * myDesired) + 
                      (desiredChannel->getDesired() * otherStrength)) 
                     / (myStrength));
    }
  void startAverage(void)
    {
      myDesiredTotal = myDesired * myStrength;
      myStrengthTotal = myStrength;
    }
  void addAverage(ArActionDesiredChannel *desiredChannel)
    {
      myDesiredTotal += (desiredChannel->getDesired() * 
                         desiredChannel->getStrength());
      myStrengthTotal += desiredChannel->getStrength();
    }
  void endAverage(void)
    {
      if (myStrengthTotal < MIN_STRENGTH)
      {
        myStrength = NO_STRENGTH;
        return;
      }
      myDesired = (myDesiredTotal / myStrengthTotal);
      myStrength = myStrengthTotal;
      if (myStrength > MAX_STRENGTH)
        myStrength = MAX_STRENGTH;
    }
protected:
  double myDesired;
  double myStrength;
  double myDesiredTotal;
  double myStrengthTotal;
};


class ArActionDesired
{
public:
  static const double NO_STRENGTH;
  static const double MIN_STRENGTH;
  static const double MAX_STRENGTH;
  ArActionDesired() { myHeadingSet = false; }
  virtual ~ArActionDesired() {}
  

  virtual void setVel(double vel, double strength = MAX_STRENGTH)
    { myVelDes.setDesired(vel, strength); }

  virtual void setDeltaHeading(double deltaHeading, double strength = MAX_STRENGTH)
    { myDeltaHeadingDes.setDesired(deltaHeading, strength); }

  virtual void setHeading(double heading, double strength = MAX_STRENGTH)
    { myHeading = heading; myHeadingStrength = strength; myHeadingSet = true; }


  virtual void setRotVel(double rotVel, double strength = MAX_STRENGTH)
    { myRotVelDes.setDesired(rotVel, strength); }



  virtual void setMaxVel(double maxVel, double strength = MAX_STRENGTH)
    { myMaxVelDes.setDesired(maxVel, strength); }

  virtual void setMaxNegVel(double maxVel, double strength = MAX_STRENGTH)
    { myMaxNegVelDes.setDesired(maxVel, strength); }


  virtual void setTransAccel(double transAccel, double strength = MAX_STRENGTH)
    { myTransAccelDes.setDesired(transAccel, strength);  }


  virtual void setTransDecel(double transDecel, double strength = MAX_STRENGTH)
    { myTransDecelDes.setDesired(transDecel, strength);  }


  virtual void setMaxRotVel(double maxVel, double strength = MAX_STRENGTH)
    { myMaxRotVelDes.setDesired(maxVel, strength); }


  virtual void setRotAccel(double rotAccel, double strength = MAX_STRENGTH)
    { myRotAccelDes.setDesired(rotAccel, strength);  }


  virtual void setRotDecel(double rotDecel, double strength = MAX_STRENGTH)
    { myRotDecelDes.setDesired(rotDecel, strength);  }

  virtual void reset(void) 
    {
      myVelDes.reset(); myDeltaHeadingDes.reset(); 
      myMaxVelDes.reset(); myMaxNegVelDes.reset(); myMaxRotVelDes.reset(); 
      myRotVelDes.reset();
      myTransAccelDes.reset(); myTransDecelDes.reset();
      myRotAccelDes.reset(); myRotDecelDes.reset();
      myHeadingSet = false;
    }

  virtual double getVel(void)  
    { return myVelDes.getDesired(); }
  virtual double getVelStrength(void) 
    { return myVelDes.getStrength(); }
  virtual double getHeading(void)
    { return myHeading; }
  virtual double getHeadingStrength(void)
    { return myHeadingStrength; }
  virtual double getDeltaHeading(void)
    { return myDeltaHeadingDes.getDesired(); }
  virtual double getDeltaHeadingStrength(void)
    { return myDeltaHeadingDes.getStrength(); }
  virtual double getRotVelStrength(void) { return myRotVelDes.getStrength(); }
  virtual double getRotVel(void) { return myRotVelDes.getDesired(); }


  virtual double getMaxVel(void)
    { return myMaxVelDes.getDesired(); }
  virtual double getMaxVelStrength(void)
    { return myMaxVelDes.getStrength(); }
  virtual double getMaxNegVel(void)
    { return myMaxNegVelDes.getDesired(); }
  virtual double getMaxNegVelStrength(void)
    { return myMaxNegVelDes.getStrength(); }
  virtual double getTransAccel(void)
    { return myTransAccelDes.getDesired(); }
  virtual double getTransAccelStrength(void)
    { return myTransAccelDes.getStrength(); }
  virtual double getTransDecel(void)
    { return myTransDecelDes.getDesired(); }
  virtual double getTransDecelStrength(void)
    { return myTransDecelDes.getStrength(); }

  virtual double getMaxRotVel(void)
    { return myMaxRotVelDes.getDesired(); }
  virtual double getMaxRotVelStrength(void)
    { return myMaxRotVelDes.getStrength(); }
  virtual double getRotAccel(void)
    { return myRotAccelDes.getDesired(); }
  virtual double getRotAccelStrength(void)
    { return myRotAccelDes.getStrength(); }
  virtual double getRotDecel(void)
    { return myRotDecelDes.getDesired(); }
  virtual double getRotDecelStrength(void)
    { return myRotDecelDes.getStrength(); }


  virtual void merge(ArActionDesired *actDesired)
    {
      if (actDesired == NULL)
        return;
      myVelDes.merge(&actDesired->myVelDes);
      // if we're already using rot or delt use that, otherwise use what it wants
      if (myDeltaHeadingDes.getStrength() > NO_STRENGTH)

      {
        myDeltaHeadingDes.merge(&actDesired->myDeltaHeadingDes);
      }
      else if (myRotVelDes.getStrength() > NO_STRENGTH)
      {
        myRotVelDes.merge(&actDesired->myRotVelDes);
      }
      else
      {
        myDeltaHeadingDes.merge(&actDesired->myDeltaHeadingDes);
        myRotVelDes.merge(&actDesired->myRotVelDes);
      }
      myMaxVelDes.merge(&actDesired->myMaxVelDes);
      myMaxNegVelDes.merge(&actDesired->myMaxNegVelDes);
      myMaxRotVelDes.merge(&actDesired->myMaxRotVelDes);
      myTransAccelDes.merge(&actDesired->myTransAccelDes);
      myTransDecelDes.merge(&actDesired->myTransDecelDes);
      myRotAccelDes.merge(&actDesired->myRotAccelDes);
      myRotDecelDes.merge(&actDesired->myRotDecelDes);
    }

  virtual void startAverage(void)
    {
      myVelDes.startAverage();
      myRotVelDes.startAverage();
      myDeltaHeadingDes.startAverage();
      myMaxVelDes.startAverage();
      myMaxNegVelDes.startAverage();
      myMaxRotVelDes.startAverage();
      myTransAccelDes.startAverage();
      myTransDecelDes.startAverage();
      myRotAccelDes.startAverage();
      myRotDecelDes.startAverage();
    }

  virtual void addAverage(ArActionDesired *actDesired)
    {
      if (actDesired == NULL)
        return;
      myVelDes.addAverage(&actDesired->myVelDes);
      // if we're using one of rot or delta heading use that,
      // otherwise use whatever they're using
      if (myRotVelDes.getStrength() > NO_STRENGTH)
      {
        myRotVelDes.addAverage(
                &actDesired->myRotVelDes);
      }
      else if (myDeltaHeadingDes.getStrength() > NO_STRENGTH)
      {
        myDeltaHeadingDes.addAverage(
                &actDesired->myDeltaHeadingDes);
      }
      else
      {
        myRotVelDes.addAverage(
                &actDesired->myRotVelDes);
        myDeltaHeadingDes.addAverage(
                &actDesired->myDeltaHeadingDes);
      }

      myMaxVelDes.addAverage(&actDesired->myMaxVelDes);
      myMaxNegVelDes.addAverage(&actDesired->myMaxNegVelDes);
      myMaxRotVelDes.addAverage(&actDesired->myMaxRotVelDes);
      myTransAccelDes.addAverage(&actDesired->myTransAccelDes);
      myTransDecelDes.addAverage(&actDesired->myTransDecelDes);
      myRotAccelDes.addAverage(&actDesired->myRotAccelDes);
      myRotDecelDes.addAverage(&actDesired->myRotDecelDes);
    }

  virtual void endAverage(void)
    {
      myVelDes.endAverage();
      myRotVelDes.endAverage();
      myDeltaHeadingDes.endAverage();
      myMaxVelDes.endAverage();
      myMaxNegVelDes.endAverage();
      myMaxRotVelDes.endAverage();
      myTransAccelDes.endAverage();
      myTransDecelDes.endAverage();
      myRotAccelDes.endAverage();
      myRotDecelDes.endAverage();
    }

  virtual void accountForRobotHeading(double robotHeading)
    {
      if (myHeadingSet)
        setDeltaHeading(ArMath::subAngle(myHeading, robotHeading), 
                        myHeadingStrength);
      myHeadingSet = false;
    }
protected:
  double myHeading;
  double myHeadingStrength;
  bool myHeadingSet;
  ArActionDesiredChannel myRotVelDes;
  ArActionDesiredChannel myVelDes;
  ArActionDesiredChannel myDeltaHeadingDes;
  ArActionDesiredChannel myMaxVelDes;
  ArActionDesiredChannel myMaxNegVelDes;
  ArActionDesiredChannel myMaxRotVelDes;
  ArActionDesiredChannel myTransAccelDes;
  ArActionDesiredChannel myTransDecelDes;
  ArActionDesiredChannel myRotAccelDes;
  ArActionDesiredChannel myRotDecelDes;
};


#endif

---