sr_deadband.hpp
Go to the documentation of this file.
00001 
00026 #ifndef _SR_DEADBAND_HPP_
00027 #define _SR_DEADBAND_HPP_
00028 
00029 #include <sr_utilities/sr_math_utils.hpp>
00030 
00031 namespace sr_deadband
00032 {
00043   template <class T>
00044   static inline bool simple_deadband (T value, T deadband)
00045   {
00046     return ( fabs(value) > deadband );
00047   }
00048 
00049   template <class T>
00050   class HysteresisDeadband
00051   {
00052   public:
00059     HysteresisDeadband() :
00060       last_demand(static_cast<T>(0.0)), entered_small_deadband(false)
00061     {
00062     };
00063 
00064     ~HysteresisDeadband()
00065     {};
00066 
00081     bool is_in_deadband(T demand, T error, T deadband, double deadband_multiplicator = 5.0, unsigned int nb_errors_for_avg = 50)
00082     {
00083       bool is_in_deadband = false;
00084 
00085       last_errors.push_back( error );
00086       double avg_error = 0.0;
00087       for( unsigned int i = 0 ; i < last_errors.size(); ++i )
00088       {
00089         avg_error += last_errors[i];
00090       }
00091       avg_error /= last_errors.size();
00092 
00093       // Received a new command:
00094       if( last_demand != demand )
00095       {
00096         entered_small_deadband = false;
00097         last_demand = demand;
00098       }
00099       else
00100       {
00101         //check if we entered the small deadband
00102         if( !entered_small_deadband )
00103         {
00104           entered_small_deadband = fabs(avg_error) < deadband;
00105         }
00106 
00107         //we always compute the error if we haven't entered the small deadband
00108         if (!entered_small_deadband)
00109           is_in_deadband = false;
00110         else
00111         {
00112           if( fabs(avg_error) > deadband_multiplicator*deadband ) //we're outside of the big deadband -> compute the error
00113           {
00114             is_in_deadband = false;
00115             //when we leave the big deadband we wait until we're back in the small deadband before stopping the motor
00116             entered_small_deadband = false;
00117           }
00118           else                                 //we're in the big deadband -> send a force demand of 0.0
00119             is_in_deadband = true;
00120         }
00121       }
00122 
00123       if( last_errors.size() > nb_errors_for_avg )
00124         last_errors.pop_front();
00125 
00126       return is_in_deadband;
00127     };
00128 
00129   private:
00130     T deadband;
00131     T last_demand;
00132     std::deque<T> last_errors;
00133     bool entered_small_deadband;
00134   };
00135 
00136 }
00137 
00138 /* For the emacs weenies in the crowd.
00139 Local Variables:
00140    c-basic-offset: 2
00141 End:
00142 */
00143 
00144 #endif


sr_utilities
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:08:44