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";
116 unsigned char OdomControl::run(
bool e_stop_on,
bool control_on,
double commanded_vel,
double measured_vel,
double dt)
158 if (
fs_ !=
nullptr &&
fs_->is_open()){
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);
206 return (
int)round(pid_val + 125.0);
226 double p_val = error *
K_P_;
232 double avg = (fabs(vel_history[0]) + fabs(vel_history[1]) + fabs(vel_history[2])) / 3.0;
242 int test_factor = 18;
246 if (motor_speed > max)
251 if (motor_speed < min)
257 test_motor = motor_speed;
267 return (motor_speed + deadband_offset);
271 return (motor_speed - deadband_offset);
277 static double time = 0;
278 float change_in_velocity = 0;
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
double velocity_filtered_
void start(bool use_control, PidGains pid_gains, int max, int min)
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)
const int MOTOR_SPEED_MIN
double differential_error_
double velocity_commanded_
double velocity_measured_