00001 // -*- C++ -*- 00010 #ifndef INTEGRATOR_H 00011 #define INTEGRATOR_H 00012 00013 // </rtc-template> 00014 00015 #include <deque> 00016 00017 class Integrator { 00018 public: 00019 // if range = 0, integrate from 0 to t. Otherwise, integrate from t - (range - 1) * dt to t. 00020 Integrator(double _dt = 0.005, unsigned int _range = 0); 00021 ~Integrator(void); 00022 void reset(void); 00023 void setup(double _dt, unsigned int _range); 00024 void update(double _x); 00025 double calculate(void); 00026 private: 00027 double dt; // control cycle 00028 double first, sum, last; // for trapezoidal rule calculation 00029 bool init_integration_flag; // integration inited flag (true when first value is updated) 00030 unsigned int range; // integration range (from t_now - range * dt to t_now [sec]) 00031 std::deque<double> buffer; // integration data buffer 00032 }; 00033 00034 #endif // TWO_DOF_CONTROLLER_H