srh_joint_variable_pid_position_controller.hpp

Go to the documentation of this file.

#ifndef _SRH_JOINT_VARIABLE_PID_POSITION_CONTROLLER_HPP_
#define _SRH_JOINT_VARIABLE_PID_POSITION_CONTROLLER_HPP_

#include <sr_mechanism_controllers/sr_controller.hpp>
#include <sr_mechanism_controllers/sr_plain_pid.hpp>


namespace controller
{
class SrhJointVariablePidPositionController :
        public SrController
{
public:
  SrhJointVariablePidPositionController();

  bool init(ros_ethercat_model::RobotStateInterface *robot, ros::NodeHandle &n);

  virtual void starting(const ros::Time &time);

  virtual void update(const ros::Time &time, const ros::Duration &period);

  virtual void getGains(double &p, double &i, double &d, double &i_max, double &i_min);

  virtual bool resetGains(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);

  bool setGains(sr_robot_msgs::SetPidGains::Request &req, sr_robot_msgs::SetPidGains::Response &resp);

  virtual void updatePid(double, double);

private:
  boost::scoped_ptr<PlainPid> pid_controller_position_;

  double position_deadband_;

  double p_init_;
  double i_init_;
  double d_init_;
  double i_clamp_;
  double error_old_;
  double set_point_old_;
  double frequency_;
  double smoothing_velocity_min_;
  double smoothing_velocity_max_;
  double smoothing_factor_p_;
  double smoothing_factor_i_;
  double smoothing_factor_d_;

  sr_deadband::HysteresisDeadband<double> hysteresis_deadband;

  void read_parameters();

  void setCommandCB(const std_msgs::Float64ConstPtr &msg);

  void resetJointState();
};
}  // namespace controller

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/


#endif