controller.h
Go to the documentation of this file.
1 /***************************************************************************/
36 #ifndef CONTROLLER_H
37 #define CONTROLLER_H
38 
39 namespace pid_ns
40 {
41 class PidObject
42 {
43 public:
44  PidObject();
45 
46 private:
47  void doCalcs();
48  void getParams(double in, double& value, double& scale);
49  void pidEnableCallback(const std_msgs::Bool& pid_enable_msg);
50  void plantStateCallback(const std_msgs::Float64& state_msg);
51  void printParameters();
52  void reconfigureCallback(pid::PidConfig& config, uint32_t level);
53  void setpointCallback(const std_msgs::Float64& setpoint_msg);
54  bool validateParameters();
55 
56  // Primary PID controller input & output variables
57  double plant_state_; // current output of plant
58  double control_effort_ = 0; // output of pid controller
59  double setpoint_ = 0; // desired output of plant
60  bool pid_enabled_ = true; // PID is enabled to run
61  bool new_state_or_setpt_ = false; // Indicate that fresh calculations need to be run
62 
65  bool first_reconfig_ = true;
66 
67  double error_integral_ = 0;
68  double proportional_ = 0; // proportional term of output
69  double integral_ = 0; // integral term of output
70  double derivative_ = 0; // derivative term of output
71 
72  // PID gains
73  double Kp_ = 0, Ki_ = 0, Kd_ = 0;
74 
75  // Parameters for error calc. with disconinuous input
76  bool angle_error_ = false;
77  double angle_wrap_ = 2.0 * 3.14159;
78 
79  // Cutoff frequency for the derivative calculation in Hz.
80  // Negative -> Has not been set by the user yet, so use a default.
81  double cutoff_frequency_ = -1;
82 
83  // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency
84  // at
85  // 1/4 of the sample rate.
86  double c_ = 1.;
87 
88  // Used to check for tan(0)==>NaN in the filter calculation
89  double tan_filt_ = 1.;
90 
91  // Upper and lower saturation limits
92  double upper_limit_ = 1000, lower_limit_ = -1000;
93 
94  // Anti-windup term. Limits the absolute value of the integral term.
95  double windup_limit_ = 1000;
96 
97  // Initialize filter data with zeros
99 
100  // Topic and node names and message objects
103 
104  std_msgs::Float64 control_msg_, state_msg_;
105 
106  // Diagnostic objects
109 };
110 } // end pid namespace
111 
112 #endif
ros::Duration delta_t_
Definition: controller.h:64
std::string topic_from_controller_
Definition: controller.h:102
std::vector< double > filtered_error_deriv_
Definition: controller.h:98
ros::Publisher control_effort_pub_
Definition: controller.h:101
double max_loop_frequency_
Definition: controller.h:107
std::vector< double > error_deriv_
Definition: controller.h:98
double plant_state_
Definition: controller.h:57
double windup_limit_
Definition: controller.h:95
double min_loop_frequency_
Definition: controller.h:107
std::string topic_from_plant_
Definition: controller.h:102
std::string pid_enable_topic_
Definition: controller.h:102
std::string setpoint_topic_
Definition: controller.h:102
bool new_state_or_setpt_
Definition: controller.h:61
double upper_limit_
Definition: controller.h:92
double error_integral_
Definition: controller.h:67
bool validateParameters()
Definition: pid.cpp:126
std_msgs::Float64 state_msg_
Definition: controller.h:104
double derivative_
Definition: controller.h:70
ros::Time prev_time_
Definition: controller.h:63
void reconfigureCallback(pid::PidConfig &config, uint32_t level)
Definition: pid.cpp:157
double cutoff_frequency_
Definition: controller.h:81
double angle_wrap_
Definition: controller.h:77
double lower_limit_
Definition: controller.h:92
int measurements_received_
Definition: controller.h:108
double control_effort_
Definition: controller.h:58
void doCalcs()
Definition: pid.cpp:174
std::vector< double > filtered_error_
Definition: controller.h:98
void pidEnableCallback(const std_msgs::Bool &pid_enable_msg)
Definition: pid.cpp:96
void setpointCallback(const std_msgs::Float64 &setpoint_msg)
Definition: pid.cpp:82
std_msgs::Float64 control_msg_
Definition: controller.h:104
void printParameters()
Definition: pid.cpp:138
void getParams(double in, double &value, double &scale)
Definition: pid.cpp:101
double proportional_
Definition: controller.h:68
void plantStateCallback(const std_msgs::Float64 &state_msg)
Definition: pid.cpp:89
std::vector< double > error_
Definition: controller.h:98


pid
Author(s): Andy Zelenak , Paul Bouchier
autogenerated on Mon Apr 15 2019 02:41:32