odom_control.cpp
Go to the documentation of this file.
1 #include <ctime>
2 #include <vector>
3 #include <string>
4 #include <cmath>
5 #include <fstream>
6 #include <iostream>
7 #include "ros/ros.h"
8 
11 
12 namespace openrover
13 {
14 void OdomControl::start(bool use_control, PidGains pid_gains, int max, int min)
15 {
16  K_P_ = pid_gains.Kp;
17  K_I_ = pid_gains.Ki;
18  K_D_ = pid_gains.Kd;
19 }
20 
24  , MOTOR_DEADBAND_(9)
25  , MAX_ACCEL_CUTOFF_(5.0)
26  , MIN_VELOCITY_(0.03)
27  , MAX_VELOCITY_(5)
28  , fs_(nullptr)
29  , K_P_(0)
30  , K_I_(0)
31  , K_D_(0)
33  , velocity_history_(3, 0)
34  , use_control_(false)
35  , skip_measurement_(false)
36  , at_max_motor_speed_(false)
37  , at_min_motor_speed_(false)
38  , stop_integrating_(false)
39  , velocity_error_(0)
40  , integral_error_(0)
45 {
46  ROS_INFO("odom Kp: %f", K_P_);
47  ROS_INFO("odom Ki: %f", K_I_);
48  ROS_INFO("odom Kd: %f", K_D_);
49 }
50 
51 OdomControl::OdomControl(bool use_control, PidGains pid_gains, int max, int min, std::ofstream* fs)
52  : MOTOR_MAX_(max)
53  , MOTOR_MIN_(min)
54  , MOTOR_DEADBAND_(9)
55  , MAX_ACCEL_CUTOFF_(5.0)
56  , MIN_VELOCITY_(0.03)
57  , MAX_VELOCITY_(3)
58  , fs_(fs)
59  , K_P_(pid_gains.Kp)
60  , K_I_(pid_gains.Ki)
61  , K_D_(pid_gains.Kd)
63  , velocity_history_(3, 0)
64  , use_control_(use_control)
65  , skip_measurement_(false)
66  , at_max_motor_speed_(false)
67  , at_min_motor_speed_(false)
68  , stop_integrating_(false)
69  , velocity_error_(0)
70  , integral_error_(0)
75 {
76  ROS_INFO("odom Kp: %f", K_P_);
77  ROS_INFO("odom Ki: %f", K_I_);
78  ROS_INFO("odom Kd: %f", K_D_);
79 
80  if (fs_ != nullptr && fs_->is_open()) {
81  *fs_ << "time,Kp,Ki,Kd,error,integral_error,differential_error,error_filtered,meas_vel,filt_vel,cmd_vel,dt,motor_cmd\n";
82  fs_->flush();
83  }
84 }
85 
86 OdomControl::OdomControl(bool use_control, PidGains pid_gains, int max, int min)
87  : MOTOR_MAX_(max)
88  , MOTOR_MIN_(min)
89  , MOTOR_DEADBAND_(9)
90  , MAX_ACCEL_CUTOFF_(5.0)
91  , MIN_VELOCITY_(0.03)
92  , MAX_VELOCITY_(3)
93  , fs_(nullptr)
94  , K_P_(pid_gains.Kp)
95  , K_I_(pid_gains.Ki)
96  , K_D_(pid_gains.Kd)
98  , velocity_history_(3, 0)
99  , use_control_(use_control)
100  , skip_measurement_(false)
101  , at_max_motor_speed_(false)
102  , at_min_motor_speed_(false)
103  , stop_integrating_(false)
104  , velocity_error_(0)
105  , integral_error_(0)
108  , velocity_measured_(0)
109  , velocity_filtered_(0)
110 {
111  ROS_INFO("odom Kp: %f", K_P_);
112  ROS_INFO("odom Ki: %f", K_I_);
113  ROS_INFO("odom Kd: %f", K_D_);
114 }
115 
116 unsigned char OdomControl::run(bool e_stop_on, bool control_on, double commanded_vel, double measured_vel, double dt)
117 {
118  velocity_commanded_ = commanded_vel;
119 
120  velocity_measured_ = measured_vel;
121 
122  velocity_filtered_ = filter(measured_vel, dt);
123 
124  //If rover is E-Stopped, respond with NEUTRAL comman
125  if (e_stop_on)
126  {
127  reset();
128  return MOTOR_NEUTRAL;
129  }
130 
131  // If stopping, stop now when velocity has slowed.
132  if ((commanded_vel == 0.0) && (fabs(velocity_filtered_)<0.3))
133  {
134  integral_error_ = 0;
136  {
137  return MOTOR_NEUTRAL;
138  }
139  }
140 
141  // If controller should be ON, run it.
142  if (control_on)
143  {
144  velocity_error_ = commanded_vel - velocity_filtered_;
145  if (!skip_measurement_)
146  {
148  ROS_DEBUG("PID: %i", motor_speed_);
149  }
150  }
151  else
152  {
154  }
155 
157 
158  if (fs_ != nullptr && fs_->is_open()){
159  *fs_ << ros::Time::now() << ",";
160  *fs_ << K_P_ << "," << K_I_ << "," << K_D_ << ",";
161  *fs_ << commanded_vel - measured_vel << "," << integral_error_ << "," << differential_error_<< "," << velocity_error_ << ",";
162  *fs_ << measured_vel << "," << velocity_filtered_ << "," << commanded_vel << ",";
163  *fs_ << dt << "," << motor_speed_ << "\n";
164  fs_->flush();
165  }
166 
167  return (unsigned char)motor_speed_;
168 }
169 
171 {
172  return (int)round(velocity_commanded_ * 50 + MOTOR_NEUTRAL);
173 }
174 
176 {
177  integral_error_ = 0;
178  velocity_error_ = 0;
180  velocity_measured_ = 0;
181  velocity_filtered_ = 0;
182  std::fill(velocity_filtered_history_.begin(), velocity_filtered_history_.end(), 0);
184  skip_measurement_ = false;
185 }
186 
187 int OdomControl::PID(double error, double dt)
188 {
189  double p_val = P(error, dt);
190  double i_val = I(error, dt);
191  double d_val = D(error, dt);
192  double pid_val = p_val + i_val + d_val;
193  ROS_DEBUG("\nerror: %lf\n dt: %lf", error, dt);
194  ROS_DEBUG("\n kp: %lf \n ki: %lf \n kd: %lf \n", p_val, i_val, d_val);
195 
196  if (fabs(pid_val) > (MOTOR_MAX_/2.0))
197  //Only integrate if the motor's aren't already at full speed
198  {
199  stop_integrating_ = true;
200  }
201  else
202  {
203  stop_integrating_ = false;
204  }
205 
206  return (int)round(pid_val + 125.0);
207 }
208 
209 double OdomControl::D(double error, double dt)
210 {
212  return K_D_ * differential_error_;
213 }
214 
215 double OdomControl::I(double error, double dt)
216 {
217  if (!stop_integrating_)
218  {
219  integral_error_ += error * dt;
220  }
221  return K_I_ * integral_error_;
222 }
223 
224 double OdomControl::P(double error, double dt)
225 {
226  double p_val = error * K_P_;
227  return error * K_P_;
228 }
229 
230 bool OdomControl::hasZeroHistory(const std::vector<double>& vel_history)
231 {
232  double avg = (fabs(vel_history[0]) + fabs(vel_history[1]) + fabs(vel_history[2])) / 3.0;
233  if (avg < 0.03)
234  return true;
235  else
236  return false;
237 }
238 
239 int OdomControl::boundMotorSpeed(int motor_speed, int max, int min)
240 {
241  int test_motor;
242  int test_factor = 18;
243  at_max_motor_speed_ = false;
244  at_min_motor_speed_ = false;
245 
246  if (motor_speed > max)
247  {
248  motor_speed = max;
249  at_max_motor_speed_ = true;
250  }
251  if (motor_speed < min)
252  {
253  motor_speed = min;
254  at_min_motor_speed_ = true;
255  }
256 
257  test_motor = motor_speed;
258 
259  return test_motor;
260 }
261 
262 int OdomControl::deadbandOffset(int motor_speed, int deadband_offset)
263 {
264  // Compensate for deadband
265  if (motor_speed > MOTOR_NEUTRAL)
266  {
267  return (motor_speed + deadband_offset);
268  }
269  else if (motor_speed < MOTOR_NEUTRAL)
270  {
271  return (motor_speed - deadband_offset);
272  }
273 }
274 
275 double OdomControl::filter(double velocity, double dt)
276 {
277  static double time = 0;
278  float change_in_velocity = 0;
279 
280  // Check for impossible acceleration, if it is impossible, ignore the measurement.
281  float accel = (velocity - velocity_filtered_history_[0]) / dt;
282  velocity_history_.insert(velocity_history_.begin(), velocity);
283  velocity_history_.pop_back();
284 
285  if (accel > MAX_ACCEL_CUTOFF_)
286  {
287  change_in_velocity = 0.5*dt*MAX_ACCEL_CUTOFF_;
288  velocity = velocity_filtered_history_[0] + change_in_velocity;
289  }
290  else if (accel < -MAX_ACCEL_CUTOFF_)
291  {
292  change_in_velocity = -0.5*dt*MAX_ACCEL_CUTOFF_;
293  velocity = velocity_filtered_history_[0] + change_in_velocity;
294  }
295 
296  // Hanning low pass filter filter
297  // velocity_filtered_ = 0.25 * velocity + 0.5 * velocity_filtered_history_[0] + 0.25 * velocity_filtered_history_[1];
300  velocity_filtered_history_.pop_back();
301 
302  return velocity_filtered_;
303 }
304 
305 
306 } // namespace openrover
bool hasZeroHistory(const std::vector< double > &vel_history)
int deadbandOffset(int motor_speed, int deadband_offset)
double D(double error, double dt)
std::vector< double > velocity_history_
int PID(double error, double dt)
int boundMotorSpeed(int motor_speed, int max, int min)
double I(double error, double dt)
const int MOTOR_SPEED_MAX
Definition: constants.hpp:55
#define ROS_INFO(...)
void start(bool use_control, PidGains pid_gains, int max, int min)
const int MOTOR_NEUTRAL
Definition: constants.hpp:54
std::vector< double > velocity_filtered_history_
double P(double error, double dt)
double filter(double left_motor_vel, double dt)
unsigned char run(bool e_stop_on, bool control_on, double commanded_vel, double measured_vel, double dt)
std::ofstream * fs_
static Time now()
const int MOTOR_SPEED_MIN
Definition: constants.hpp:56
#define ROS_DEBUG(...)


rr_openrover_driver
Author(s): Jack Kilian
autogenerated on Thu Sep 10 2020 03:38:38