Skip to content
Snippets Groups Projects
Commit 7f033909 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a speed_deadband parameter.

parent a5127167
No related branches found
No related tags found
1 merge request!1Development
......@@ -43,6 +43,7 @@ gen.add("speed_Kp", double_t, 0, "Proportional constant of th
gen.add("speed_Ki", double_t, 0, "Integral constant of the speed PID", 0.0, 0.0, 1000.0)
gen.add("speed_Kd", double_t, 0, "Differential constant of the speed PID", 0.0, 0.0, 1000.0)
gen.add("speed_i_max", double_t, 0, "Maximum integral value pf the speed PID", 10.0, 0.0, 1000.0)
gen.add("speed_deadband", double_t, 0, "Minimum valid speed command", 0.1, 0.1, 1.0)
gen.add("watchdog_time", double_t, 0, "Maximum time between two control commands", 0.5, 0.0, 1000.0)
gen.add("axel_distance", double_t, 0, "Distance between the two axels in meters", 0.3662, 0.0, 10.0)
gen.add("max_steer_angle", double_t, 0, "Maximum steering angle", 0.4, -1.0,1.0)
......
......@@ -3,6 +3,7 @@ speed_Kp: 20.0
speed_Ki: 10.0
speed_Kd: 0.05
speed_i_max: 20.0
speed_deadband: 0.1
watchdog_time: 0.5
axel_distance: 0.3662
max_steer_angle: 0.4
......
......@@ -3,6 +3,7 @@ speed_Kp: 20.0
speed_Ki: 10.0
speed_Kd: 0.05
speed_i_max: 20.0
speed_deadband: 0.1
watchdog_time: 0.5
axel_distance: 0.3662
max_steer_angle: 0.4
......
......@@ -3,6 +3,7 @@ speed_Kp: 20.0
speed_Ki: 10.0
speed_Kd: 0.05
speed_i_max: 20.0
speed_deadband: 0.1
watchdog_time: 0.5
axel_distance: 0.3662
max_steer_angle: 0.4
......
......@@ -76,7 +76,7 @@ void ModelCarControlAlgNode::mainNodeThread(void)
}
else
{
if(this->linear_speed_control==0.0)
if(fabs(this->linear_speed_control)<this->config_.speed_deadband)
{
this->control_AckermannDriveStamped_msg_.drive.speed=0.0;
this->speed_pid.reset();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment