sr_deadband.hpp

Go to the documentation of this file.

/*
* Copyright 2011 Shadow Robot Company Ltd.
*
* 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 version 2 of the License.
*
* 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, see <http://www.gnu.org/licenses/>.
*/

#ifndef _SR_DEADBAND_HPP_
#define _SR_DEADBAND_HPP_

#include <sr_utilities/sr_math_utils.hpp>
#include <deque>

namespace sr_deadband
{
template<class T>
static inline bool simple_deadband(T value, T deadband)
{
  return (fabs(value) > deadband);
}

template<class T>
class HysteresisDeadband
{
public:
  HysteresisDeadband() :
          last_demand(static_cast<T>(0.0)), entered_small_deadband(false)
  {
  };

  ~HysteresisDeadband()
  {
  };

  bool is_in_deadband(T demand, T error, T deadband,
                      double deadband_multiplicator = 5.0,
                      unsigned int nb_errors_for_avg = 50)
  {
    bool is_in_deadband = false;

    last_errors.push_back(error);
    double avg_error = 0.0;
    for (unsigned int i = 0; i < last_errors.size(); ++i)
    {
      avg_error += last_errors[i];
    }
    avg_error /= last_errors.size();

    // Received a new command:
    if (last_demand != demand)
    {
      entered_small_deadband = false;
      last_demand = demand;
    }
    else
    {
      // check if we entered the small deadband
      if (!entered_small_deadband)
      {
        entered_small_deadband = fabs(avg_error) < deadband;
      }

      // we always compute the error if we haven't entered the small deadband
      if (!entered_small_deadband)
      {
        is_in_deadband = false;
      }
      else
      {
        if (fabs(avg_error) > deadband_multiplicator * deadband)
        {
          // we're outside of the big deadband -> compute the error
          is_in_deadband = false;
          // when we leave the big deadband we wait until we're back in the small deadband before stopping the motor
          entered_small_deadband = false;
        }
        else
        {
          // we're in the big deadband -> send a force demand of 0.0
          is_in_deadband = true;
        }
      }
    }

    if (last_errors.size() > nb_errors_for_avg)
    {
      last_errors.pop_front();
    }

    return is_in_deadband;
  };

private:
  T deadband;
  T last_demand;
  std::deque<T> last_errors;
  bool entered_small_deadband;
};

}  // namespace sr_deadband

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/

#endif