autotune.cpp
Go to the documentation of this file.
1 /***************************************************************************/
36 // Use the Ziegler Nichols Method to calculate reasonable PID gains.
37 // The ZN Method is based on setting Ki & Kd to zero, then cranking up Kp until
38 // oscillations are observed.
39 // This node varies Kp through a range until oscillations are just barely
40 // observed,
41 // then calculates the other parameters automatically.
42 // See https://en.wikipedia.org/wiki/PID_controller
43 
44 #include <math.h>
45 #include <ros/ros.h>
46 #include <std_msgs/Float64.h>
47 #include <string>
48 
49 // Use dynamic_reconfigure to adjust Kp, Ki, and Kd
50 #include <dynamic_reconfigure/Config.h>
51 #include <dynamic_reconfigure/DoubleParameter.h>
52 #include <dynamic_reconfigure/Reconfigure.h>
53 
54 void setKiKdToZero();
55 void setKp(double Kp);
56 void setpointCallback(const std_msgs::Float64& setpoint_msg);
57 void stateCallback(const std_msgs::Float64& state_msg);
58 void setFinalParams();
59 
60 namespace autotune
61 {
62 double Ku = 0.;
63 double Tu = 0.;
64 double setpoint = 0.;
65 double state = 0.;
66 std::string ns = "/left_wheel_pid/";
68 int num_loops = 100; // Will look for oscillations for num_loops*loopRate
69 int initial_error = 0;
70 double Kp_ZN = 0.;
71 double Ki_ZN = 0.;
72 double Kd_ZN = 0.;
73 bool found_Ku = false;
74 std::vector<double> oscillation_times(3); // Used to calculate Tu, the oscillation period
75 }
76 
77 int main(int argc, char** argv)
78 {
79  ros::init(argc, argv, "autotune_node");
80  ros::NodeHandle autotune_node;
81  ros::start();
82  ros::Subscriber setpoint_sub = autotune_node.subscribe("/setpoint", 1, setpointCallback);
83  ros::Subscriber state_sub = autotune_node.subscribe("/state", 1, stateCallback);
84  ros::Rate loopRate(50);
85 
86  // Set Ki and Kd to zero for the ZN method with dynamic_reconfigure
87  setKiKdToZero();
88 
89  // Define how rapidly the value of Kp is varied, and the max/min values to try
90  double Kp_max = 10.;
91  double Kp_min = 0.5;
92  double Kp_step = 1.0;
93 
94  for (double Kp = Kp_min; Kp <= Kp_max; Kp += Kp_step)
95  {
97  // Get the error sign.
99  // Need to wait for new setpoint/state msgs
100  ros::topic::waitForMessage<std_msgs::Float64>("setpoint");
101  ros::topic::waitForMessage<std_msgs::Float64>("state");
102 
103  // Try a new Kp.
104  setKp(Kp);
105  ROS_INFO_STREAM("Trying Kp = " << Kp); // Blank line on terminal
106  autotune::oscillation_count = 0; // Reset to look for oscillations again
107 
108  for (int i = 0; i < autotune::num_loops; i++) // Collect data for loopRate*num_loops seconds
109  {
110  ros::spinOnce();
111  loopRate.sleep();
112  if (i == 0) // Get the sign of the initial error
113  {
115  }
116 
117  // Did the system oscillate about the setpoint? If so, Kp~Ku.
118  // Oscillation => the sign of the error changes
119  // The first oscillation is caused by the change in setpoint. Ignore it.
120  // Look for 2 oscillations.
121  // Get a fresh state message
122  ros::topic::waitForMessage<std_msgs::Float64>("state");
123  double new_error = (autotune::setpoint - autotune::state); // Sign of the error
124  // ROS_INFO_STREAM("New error: "<< new_error);
125  if (std::signbit(autotune::initial_error) != std::signbit(new_error))
126  {
128  loopRate.expectedCycleTime().toSec() * i; // Record the time to calculate a period, Tu
130  ROS_INFO_STREAM("Oscillation occurred. Oscillation count: " << autotune::oscillation_count);
131  autotune::initial_error = new_error; // Reset to look for another oscillation
132 
133  // If the system is definitely oscillating about the setpoint
135  {
136  // Now calculate the period of oscillation (Tu)
138  ROS_INFO_STREAM("Tu (oscillation period): " << autotune::Tu);
139  // ROS_INFO_STREAM( "2*sampling period: " <<
140  // 2.*loopRate.expectedCycleTime().toSec() );
141 
142  // We're looking for more than just the briefest dip across the
143  // setpoint and back.
144  // Want to see significant oscillation
145  if (autotune::Tu > 3. * loopRate.expectedCycleTime().toSec())
146  {
147  autotune::Ku = Kp;
148 
149  // Now calculate the other parameters with ZN method
153 
154  autotune::found_Ku = true;
155  goto DONE;
156  }
157  else
158  break; // Try the next Kp
159  }
160  }
161  }
162  }
163 DONE:
164 
165  if (autotune::found_Ku == true)
166  {
167  setFinalParams();
168  }
169  else
170  ROS_INFO_STREAM("Did not see any oscillations for this range of Kp. Adjust "
171  "Kp_max and Kp_min to broaden the search.");
172 
173  ros::shutdown();
174  return 0;
175 }
176 
177 // Set Ki and Kd to zero with dynamic_reconfigure
179 {
180  dynamic_reconfigure::ReconfigureRequest srv_req;
181  dynamic_reconfigure::ReconfigureResponse srv_resp;
182  dynamic_reconfigure::DoubleParameter double_param;
183  dynamic_reconfigure::Config config;
184  double_param.name = "Ki";
185  double_param.value = 0.0;
186  config.doubles.push_back(double_param);
187  double_param.name = "Kd";
188  double_param.value = 0.0;
189  config.doubles.push_back(double_param);
190  srv_req.config = config;
191  ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp);
192 }
193 
194 // Set Kp with dynamic_reconfigure
195 void setKp(double Kp)
196 {
197  dynamic_reconfigure::ReconfigureRequest srv_req;
198  dynamic_reconfigure::ReconfigureResponse srv_resp;
199  dynamic_reconfigure::DoubleParameter double_param;
200  dynamic_reconfigure::Config config;
201 
202  // A blank service call to get the current parameters into srv_resp
203  ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp);
204 
205  double_param.name = "Kp";
206  double_param.value = Kp / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI
207  config.doubles.push_back(double_param);
208  srv_req.config = config;
209  ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp);
210 }
211 
212 void setpointCallback(const std_msgs::Float64& setpoint_msg)
213 {
214  autotune::setpoint = setpoint_msg.data;
215 }
216 
217 void stateCallback(const std_msgs::Float64& state_msg)
218 {
219  autotune::state = state_msg.data;
220 }
221 
222 // Print out and set the final parameters as calculated by the autotuner
224 {
225  ROS_INFO_STREAM(" ");
226  ROS_INFO_STREAM("The suggested parameters are: ");
230 
231  // Set the ZN parameters with dynamic_reconfigure
232  dynamic_reconfigure::ReconfigureRequest srv_req;
233  dynamic_reconfigure::ReconfigureResponse srv_resp;
234  dynamic_reconfigure::DoubleParameter double_param;
235  dynamic_reconfigure::Config config;
236 
237  // A blank service call to get the current parameters into srv_resp
238  ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp);
239 
240  double_param.name = "Kp";
241  double_param.value = autotune::Kp_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI
242  config.doubles.push_back(double_param);
243 
244  double_param.name = "Ki";
245  double_param.value = autotune::Ki_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI
246  config.doubles.push_back(double_param);
247 
248  double_param.name = "Kd";
249  double_param.value = autotune::Kd_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI
250  config.doubles.push_back(double_param);
251 
252  srv_req.config = config;
253  ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp);
254 }
void setpointCallback(const std_msgs::Float64 &setpoint_msg)
Definition: autotune.cpp:212
ROSCPP_DECL void start()
double Kp_ZN
Definition: autotune.cpp:70
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void setKiKdToZero()
Definition: autotune.cpp:178
int initial_error
Definition: autotune.cpp:69
bool call(const std::string &service_name, MReq &req, MRes &res)
double state
Definition: autotune.cpp:65
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool found_Ku
Definition: autotune.cpp:73
std::string ns
Definition: autotune.cpp:66
int num_loops
Definition: autotune.cpp:68
void setFinalParams()
Definition: autotune.cpp:223
double Ki_ZN
Definition: autotune.cpp:71
Duration expectedCycleTime() const
double Tu
Definition: autotune.cpp:63
std::vector< double > oscillation_times(3)
double setpoint
Definition: autotune.cpp:64
bool sleep()
#define ROS_INFO_STREAM(args)
void setKp(double Kp)
Definition: autotune.cpp:195
int oscillation_count
Definition: autotune.cpp:67
ROSCPP_DECL void shutdown()
int main(int argc, char **argv)
Definition: autotune.cpp:77
double Ku
Definition: autotune.cpp:62
double Kd_ZN
Definition: autotune.cpp:72
ROSCPP_DECL void spinOnce()
void stateCallback(const std_msgs::Float64 &state_msg)
Definition: autotune.cpp:217


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