sr_deadband.hpp

Go to the documentation of this file.

#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;
  };

}

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

#endif