defs.h
Go to the documentation of this file.
00001 #ifndef __DEFS_H__
00002 #define __DEFS_H__
00003 
00004 #include <math.h>
00005 
00006 enum {RLEG, LLEG};
00007 enum {FX, FY, FZ, MX, MY, MZ};
00008 enum {X, Y, Z};
00009 enum {WX, WY, WZ};
00010 enum {TOE, HEEL};
00011 enum {INSIDE, OUTSIDE};
00012 enum {RL, FB};
00013 
00014 #define sgn(x)  (((x)>0)?1:-1)
00015 #define deg2rad(x)      ((x)*M_PI/180)
00016 #define rad2deg(x)      ((x)*180/M_PI)
00017 #define sqr(x)          ((x)*(x))
00018 #define LPF(dT, omega, x, y)    ((y) = (((dT)*(omega)/(1+(dT)*(omega)))*(x)+1/(1+(dT)*(omega))*(y)))
00019 
00020 template<class T>
00021 void LIMITER(T &org, const T &min_, const T &max_)
00022 {
00023     for (unsigned int i=0; i<org.size(); i++){
00024         if (org(i) > max_(i)){
00025             org(i) = max_(i);
00026         }else if (org(i) < min_(i)){
00027             org(i) = min_(i);
00028         }
00029     }
00030 }
00031 
00032 template<class T>
00033 void DEADZONE(T &org, const T &min_, const T &max_)
00034 {
00035     for (unsigned int i=0; i<org.size(); i++){
00036         if (org(i) > max_(i)){
00037             org(i) -= max_(i);
00038         }else if (org(i) < min_(i)){
00039             org(i) -= min_(i);
00040         }else{
00041             org(i) = 0;
00042         }
00043     }
00044 }
00045 
00046 inline void LIMIT(double &org, const double max_)
00047 {
00048     if (org > max_){
00049         org = max_;
00050     }else if (org < -max_){
00051         org = -max_;
00052     }
00053 }
00054 
00055 inline void LIMITER(double &org, const double min_, const double max_)
00056 {
00057     if (org > max_){
00058         org = max_;
00059     }else if (org < min_){
00060         org = min_;
00061     }
00062 }
00063 
00064 inline void LIMITER(int &org, const int min_, const int max_)
00065 {
00066     if (org > max_){
00067         org = max_;
00068     }else if (org < min_){
00069         org = min_;
00070     }
00071 }
00072 
00073 inline void DEADZONE(double &org, const double min_, const double max_)
00074 {
00075     if (org > max_){
00076         org -= max_;
00077     }else if (org < min_){
00078         org -= min_;
00079     }else{
00080         org = 0;
00081     }
00082 }
00083 
00084 inline void HYSTERESIS(double &newv, const double old, 
00085                        const double min_, const double max_)
00086 {
00087     if (old >= newv-min_){
00088         newv -= min_;
00089     }else if (old <= newv - max_){
00090         newv -= max_;
00091     }else{
00092         newv = old;
00093     }
00094 }
00095 
00096 #endif


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:17