ctrl_pid.h

/*
 * Developer : Philippe Piatkiewitz (philippe.piatkiewitz@vectioneer.com)
 * All rights reserved. Copyright (c) 2014-2015 VECTIONEER.
 */
 
#ifndef MCX_CTRL3_PID_H
#define MCX_CTRL3_PID_H
 
#include <cmath>
#include <mcx/core.h>
 
namespace mcx {
 
namespace control3 {
 
class PID : public container::Module {
public:
  PID();
 
  ~PID() override;
 
  inline void setError(double error);
 
  inline void setActual(double actual);
 
  inline double getOutput() const;
 
  inline void setKp(double kp);
 
  inline void setKi(double ki);
 
  inline void setKd(double kd);
 
  inline void setIMax(double iMax);
 
  inline void setIReset(bool iReset);
 
private:
  void create_(const char* name, parameter_server::Parameter* parameter_server, uint64_t dt_micro_s) override{};
 
  bool initPhase1_() override;
 
  bool initPhase2_() override { return true; }
 
  bool startOp_() override { return true; }
 
  bool iterateOp_(const container::TaskTime& system_time, container::UserTime* user_time) override;
 
  bool stopOp_() override { return true; }
 
private:
  double e_{0};
  double a_{0};
  double y_{0};
  double kp_{0};
  double ki_{0};
  double kd_{0};
 
  double iMax_{0.1};
  double iMin_{-0.1};
  double iPositionErrorThresholdPos_{0};
  double iPositionErrorThresholdNeg_{0};
  double iActualVelocityThresholdPos_{std::numeric_limits<double>::infinity()};
  double iActualVelocityThresholdNeg_{-std::numeric_limits<double>::infinity()};
 
  double laste_{0};
  double lasta_{0};
  double iTerm_{0};
  double dTerm_{0};
 
  bool iReset_{false};
};
 
inline void PID::setError(double error) { e_ = error; }
 
inline void PID::setActual(double actual) { a_ = actual; }
 
inline double PID::getOutput() const { return y_; }
 
inline void PID::setKp(double kp) { kp_ = kp; }
 
inline void PID::setKi(double ki) { ki_ = ki; }
 
inline void PID::setKd(double kd) { kd_ = kd; }
 
inline void PID::setIMax(double iMax) { iMax_ = fabs(iMax); }
 
inline void PID::setIReset(bool iReset) { iReset_ = iReset; }
 
} // namespace control3
 
} // namespace mcx
 
#endif