Dave Lu / Mbed 2 deprecated FYDP

IMU and knee angle. No servo yet

Dependencies:   mbed

Fork of FYDP_Final2 by Dave Lu

Control/Smoother.hpp

Committer:
tntmarket
Date:
2015-03-25
Revision:
9:7a8fb72f9a93

File content as of revision 9:7a8fb72f9a93:

#ifndef SMOOTHER_H
#define SMOOTHER_H

#include <math.h>

class Smoother {
   float lastY;
   float lastT;
   const float A;
   const float dT;

   public:
      Smoother() : lastY(0), lastT(0), A(0.8), dT(0.05) {}

      void insert(float t, float y) {
         float decay = pow(A, (t-lastT)/dT);
         lastT = t;
         lastY = decay*lastY + (1-decay)*y;
      }

      float get() { return lastY; }
};

#endif // SMOOTHER_H