From 90d24f4d8f1f4b606ffb69f3a59bcb2fd079c7ca Mon Sep 17 00:00:00 2001 From: Hugo LEVY-FALK Date: Fri, 22 Mar 2019 16:59:55 +0100 Subject: [PATCH] Double PID --- .../detect_targets/TriangleParamConfig.h | 98 ++++++++++++++++ .../docs/TriangleParamConfig-usage.dox | 7 ++ .../docs/TriangleParamConfig.dox | 7 ++ .../docs/TriangleParamConfig.wikidoc | 110 +++++++++++------- .../detect_targets/cfg/triangle_control.cfg | 7 ++ .../scripts/triangle_control.py | 48 +++++++- 6 files changed, 233 insertions(+), 44 deletions(-) diff --git a/workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h b/workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h index b1ba32b..565e7e8 100644 --- a/workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h +++ b/workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h @@ -241,10 +241,17 @@ class DEFAULT if("target_depth"==(*_i)->name){target_depth = boost::any_cast(val);} if("distance_to_target"==(*_i)->name){distance_to_target = boost::any_cast(val);} if("max_speed"==(*_i)->name){max_speed = boost::any_cast(val);} + if("max_acceleration"==(*_i)->name){max_acceleration = boost::any_cast(val);} + if("speed_linear_x_Kp"==(*_i)->name){speed_linear_x_Kp = boost::any_cast(val);} + if("speed_linear_x_Ki"==(*_i)->name){speed_linear_x_Ki = boost::any_cast(val);} + if("speed_linear_x_Kd"==(*_i)->name){speed_linear_x_Kd = boost::any_cast(val);} if("linear_x_Kp"==(*_i)->name){linear_x_Kp = boost::any_cast(val);} if("linear_x_Ki"==(*_i)->name){linear_x_Ki = boost::any_cast(val);} if("linear_x_Kd"==(*_i)->name){linear_x_Kd = boost::any_cast(val);} if("control_linear_x"==(*_i)->name){control_linear_x = boost::any_cast(val);} + if("speed_linear_y_Kp"==(*_i)->name){speed_linear_y_Kp = boost::any_cast(val);} + if("speed_linear_y_Ki"==(*_i)->name){speed_linear_y_Ki = boost::any_cast(val);} + if("speed_linear_y_Kd"==(*_i)->name){speed_linear_y_Kd = boost::any_cast(val);} if("linear_y_Kp"==(*_i)->name){linear_y_Kp = boost::any_cast(val);} if("linear_y_Ki"==(*_i)->name){linear_y_Ki = boost::any_cast(val);} if("linear_y_Kd"==(*_i)->name){linear_y_Kd = boost::any_cast(val);} @@ -265,10 +272,17 @@ double target_width; double target_depth; double distance_to_target; double max_speed; +double max_acceleration; +double speed_linear_x_Kp; +double speed_linear_x_Ki; +double speed_linear_x_Kd; double linear_x_Kp; double linear_x_Ki; double linear_x_Kd; bool control_linear_x; +double speed_linear_y_Kp; +double speed_linear_y_Ki; +double speed_linear_y_Kd; double linear_y_Kp; double linear_y_Ki; double linear_y_Kd; @@ -300,6 +314,14 @@ bool control_angular_z; double distance_to_target; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double max_speed; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double max_acceleration; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double speed_linear_x_Kp; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double speed_linear_x_Ki; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double speed_linear_x_Kd; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double linear_x_Kp; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" @@ -308,6 +330,12 @@ bool control_angular_z; double linear_x_Kd; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" bool control_linear_x; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double speed_linear_y_Kp; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double speed_linear_y_Ki; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double speed_linear_y_Kd; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" double linear_y_Kp; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" @@ -520,6 +548,46 @@ TriangleParamConfig::GroupDescription("max_speed", "double", 0, "the maximal linear speed", "", &TriangleParamConfig::max_speed))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("max_speed", "double", 0, "the maximal linear speed", "", &TriangleParamConfig::max_speed))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.max_acceleration = 0.01; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.max_acceleration = 1.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.max_acceleration = 0.3; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("max_acceleration", "double", 0, "the maximal linear speed", "", &TriangleParamConfig::max_acceleration))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("max_acceleration", "double", 0, "the maximal linear speed", "", &TriangleParamConfig::max_acceleration))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.speed_linear_x_Kp = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.speed_linear_x_Kp = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.speed_linear_x_Kp = 0.01; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_x_Kp", "double", 0, "linear.x controller Kp", "", &TriangleParamConfig::speed_linear_x_Kp))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_x_Kp", "double", 0, "linear.x controller Kp", "", &TriangleParamConfig::speed_linear_x_Kp))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.speed_linear_x_Ki = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.speed_linear_x_Ki = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.speed_linear_x_Ki = 0.01; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_x_Ki", "double", 0, "linear.x controller Ki", "", &TriangleParamConfig::speed_linear_x_Ki))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_x_Ki", "double", 0, "linear.x controller Ki", "", &TriangleParamConfig::speed_linear_x_Ki))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.speed_linear_x_Kd = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.speed_linear_x_Kd = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.speed_linear_x_Kd = 0.01; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_x_Kd", "double", 0, "linear.x controller Kd", "", &TriangleParamConfig::speed_linear_x_Kd))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_x_Kd", "double", 0, "linear.x controller Kd", "", &TriangleParamConfig::speed_linear_x_Kd))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __min__.linear_x_Kp = 0.0; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" @@ -560,6 +628,36 @@ TriangleParamConfig::GroupDescription("control_linear_x", "bool", 0, "Control distance to target", "", &TriangleParamConfig::control_linear_x))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("control_linear_x", "bool", 0, "Control distance to target", "", &TriangleParamConfig::control_linear_x))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.speed_linear_y_Kp = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.speed_linear_y_Kp = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.speed_linear_y_Kp = 0.01; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_y_Kp", "double", 0, "linear.y controller Kp", "", &TriangleParamConfig::speed_linear_y_Kp))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_y_Kp", "double", 0, "linear.y controller Kp", "", &TriangleParamConfig::speed_linear_y_Kp))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.speed_linear_y_Ki = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.speed_linear_y_Ki = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.speed_linear_y_Ki = 0.01; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_y_Ki", "double", 0, "linear.y controller Ki", "", &TriangleParamConfig::speed_linear_y_Ki))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_y_Ki", "double", 0, "linear.y controller Ki", "", &TriangleParamConfig::speed_linear_y_Ki))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.speed_linear_y_Kd = 0.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.speed_linear_y_Kd = 2.0; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.speed_linear_y_Kd = 0.01; +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_y_Kd", "double", 0, "linear.y controller Kd", "", &TriangleParamConfig::speed_linear_y_Kd))); +//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription("speed_linear_y_Kd", "double", 0, "linear.y controller Kd", "", &TriangleParamConfig::speed_linear_y_Kd))); //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" __min__.linear_y_Kp = 0.0; //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" diff --git a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox index e236ad2..d1d0ed1 100644 --- a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox +++ b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox @@ -6,10 +6,17 @@ + + + + + + + diff --git a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox index 20fd123..677ef6d 100644 --- a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox +++ b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox @@ -7,10 +7,17 @@ Reads and maintains the following parameters on the ROS server - \b "~target_depth" : \b [double] the real target depth (m) min: 0.01, default: 0.2, max: 0.5 - \b "~distance_to_target" : \b [double] The required distance to the target (m) min: 1.0, default: 2.0, max: 5.0 - \b "~max_speed" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 1.0 +- \b "~max_acceleration" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 1.0 +- \b "~speed_linear_x_Kp" : \b [double] linear.x controller Kp min: 0.0, default: 0.01, max: 2.0 +- \b "~speed_linear_x_Ki" : \b [double] linear.x controller Ki min: 0.0, default: 0.01, max: 2.0 +- \b "~speed_linear_x_Kd" : \b [double] linear.x controller Kd min: 0.0, default: 0.01, max: 2.0 - \b "~linear_x_Kp" : \b [double] linear.x controller Kp min: 0.0, default: 0.01, max: 1.0 - \b "~linear_x_Ki" : \b [double] linear.x controller Ki min: 0.0, default: 0.01, max: 1.0 - \b "~linear_x_Kd" : \b [double] linear.x controller Kd min: 0.0, default: 0.01, max: 1.0 - \b "~control_linear_x" : \b [bool] Control distance to target min: False, default: True, max: True +- \b "~speed_linear_y_Kp" : \b [double] linear.y controller Kp min: 0.0, default: 0.01, max: 2.0 +- \b "~speed_linear_y_Ki" : \b [double] linear.y controller Ki min: 0.0, default: 0.01, max: 2.0 +- \b "~speed_linear_y_Kd" : \b [double] linear.y controller Kd min: 0.0, default: 0.01, max: 2.0 - \b "~linear_y_Kp" : \b [double] linear.y controller Kp min: 0.0, default: 0.01, max: 1.0 - \b "~linear_y_Ki" : \b [double] linear.y controller Ki min: 0.0, default: 0.01, max: 1.0 - \b "~linear_y_Kd" : \b [double] linear.y controller Kd min: 0.0, default: 0.01, max: 1.0 diff --git a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc index 74410e8..409f7ca 100644 --- a/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc +++ b/workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc @@ -23,70 +23,98 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig 4.default= 0.3 4.type= double 4.desc=the maximal linear speed Range: 0.01 to 1.0 -5.name= ~linear_x_Kp -5.default= 0.01 +5.name= ~max_acceleration +5.default= 0.3 5.type= double -5.desc=linear.x controller Kp Range: 0.0 to 1.0 -6.name= ~linear_x_Ki +5.desc=the maximal linear speed Range: 0.01 to 1.0 +6.name= ~speed_linear_x_Kp 6.default= 0.01 6.type= double -6.desc=linear.x controller Ki Range: 0.0 to 1.0 -7.name= ~linear_x_Kd +6.desc=linear.x controller Kp Range: 0.0 to 2.0 +7.name= ~speed_linear_x_Ki 7.default= 0.01 7.type= double -7.desc=linear.x controller Kd Range: 0.0 to 1.0 -8.name= ~control_linear_x -8.default= True -8.type= bool -8.desc=Control distance to target -9.name= ~linear_y_Kp +7.desc=linear.x controller Ki Range: 0.0 to 2.0 +8.name= ~speed_linear_x_Kd +8.default= 0.01 +8.type= double +8.desc=linear.x controller Kd Range: 0.0 to 2.0 +9.name= ~linear_x_Kp 9.default= 0.01 9.type= double -9.desc=linear.y controller Kp Range: 0.0 to 1.0 -10.name= ~linear_y_Ki +9.desc=linear.x controller Kp Range: 0.0 to 1.0 +10.name= ~linear_x_Ki 10.default= 0.01 10.type= double -10.desc=linear.y controller Ki Range: 0.0 to 1.0 -11.name= ~linear_y_Kd +10.desc=linear.x controller Ki Range: 0.0 to 1.0 +11.name= ~linear_x_Kd 11.default= 0.01 11.type= double -11.desc=linear.y controller Kd Range: 0.0 to 1.0 -12.name= ~control_linear_y +11.desc=linear.x controller Kd Range: 0.0 to 1.0 +12.name= ~control_linear_x 12.default= True 12.type= bool -12.desc=Controls the facing to target -13.name= ~linear_z_Kp +12.desc=Control distance to target +13.name= ~speed_linear_y_Kp 13.default= 0.01 13.type= double -13.desc=linear.z controller Kp Range: 0.0 to 10.0 -14.name= ~linear_z_Ki +13.desc=linear.y controller Kp Range: 0.0 to 2.0 +14.name= ~speed_linear_y_Ki 14.default= 0.01 14.type= double -14.desc=linear.z controller Ki Range: 0.0 to 10.0 -15.name= ~linear_z_Kd +14.desc=linear.y controller Ki Range: 0.0 to 2.0 +15.name= ~speed_linear_y_Kd 15.default= 0.01 15.type= double -15.desc=linear.z controller Kd Range: 0.0 to 10.0 -16.name= ~control_linear_z -16.default= True -16.type= bool -16.desc=Controls the facing to target -17.name= ~angular_z_Kp +15.desc=linear.y controller Kd Range: 0.0 to 2.0 +16.name= ~linear_y_Kp +16.default= 0.01 +16.type= double +16.desc=linear.y controller Kp Range: 0.0 to 1.0 +17.name= ~linear_y_Ki 17.default= 0.01 17.type= double -17.desc=angular.z controller Kp Range: 0.0 to 10.0 -18.name= ~angular_z_Ki +17.desc=linear.y controller Ki Range: 0.0 to 1.0 +18.name= ~linear_y_Kd 18.default= 0.01 18.type= double -18.desc=angular.z controller Ki Range: 0.0 to 10.0 -19.name= ~angular_z_Kd -19.default= 0.01 -19.type= double -19.desc=angular.z controller Kd Range: 0.0 to 10.0 -20.name= ~control_angular_z -20.default= True -20.type= bool -20.desc=Controls the facing to target +18.desc=linear.y controller Kd Range: 0.0 to 1.0 +19.name= ~control_linear_y +19.default= True +19.type= bool +19.desc=Controls the facing to target +20.name= ~linear_z_Kp +20.default= 0.01 +20.type= double +20.desc=linear.z controller Kp Range: 0.0 to 10.0 +21.name= ~linear_z_Ki +21.default= 0.01 +21.type= double +21.desc=linear.z controller Ki Range: 0.0 to 10.0 +22.name= ~linear_z_Kd +22.default= 0.01 +22.type= double +22.desc=linear.z controller Kd Range: 0.0 to 10.0 +23.name= ~control_linear_z +23.default= True +23.type= bool +23.desc=Controls the facing to target +24.name= ~angular_z_Kp +24.default= 0.01 +24.type= double +24.desc=angular.z controller Kp Range: 0.0 to 10.0 +25.name= ~angular_z_Ki +25.default= 0.01 +25.type= double +25.desc=angular.z controller Ki Range: 0.0 to 10.0 +26.name= ~angular_z_Kd +26.default= 0.01 +26.type= double +26.desc=angular.z controller Kd Range: 0.0 to 10.0 +27.name= ~control_angular_z +27.default= True +27.type= bool +27.desc=Controls the facing to target } } # End of autogenerated section. You may edit below. diff --git a/workspace/src/detect_targets/cfg/triangle_control.cfg b/workspace/src/detect_targets/cfg/triangle_control.cfg index e192354..082a11b 100755 --- a/workspace/src/detect_targets/cfg/triangle_control.cfg +++ b/workspace/src/detect_targets/cfg/triangle_control.cfg @@ -10,13 +10,20 @@ gen.add("target_width", double_t, 0, "the real target width (m)", 1, 0.01, 1.5) gen.add("target_depth", double_t, 0, "the real target depth (m)", .2, 0.01, 0.5) gen.add("distance_to_target", double_t, 0, "The required distance to the target (m)", 2, 1, 5) gen.add("max_speed", double_t, 0, "the maximal linear speed", .3, .01, 1) +gen.add("max_acceleration", double_t, 0, "the maximal linear speed", .3, .01, 1) +gen.add("speed_linear_x_Kp", double_t, 0, "linear.x controller Kp", .01, 0, 2) +gen.add("speed_linear_x_Ki", double_t, 0, "linear.x controller Ki", .01, 0, 2) +gen.add("speed_linear_x_Kd", double_t, 0, "linear.x controller Kd", .01, 0, 2) gen.add("linear_x_Kp", double_t, 0, "linear.x controller Kp", .01, 0, 1) gen.add("linear_x_Ki", double_t, 0, "linear.x controller Ki", .01, 0, 1) gen.add("linear_x_Kd", double_t, 0, "linear.x controller Kd", .01, 0, 1) gen.add("control_linear_x", bool_t, 0, "Control distance to target", True) +gen.add("speed_linear_y_Kp", double_t, 0, "linear.y controller Kp", .01, 0, 2) +gen.add("speed_linear_y_Ki", double_t, 0, "linear.y controller Ki", .01, 0, 2) +gen.add("speed_linear_y_Kd", double_t, 0, "linear.y controller Kd", .01, 0, 2) gen.add("linear_y_Kp", double_t, 0, "linear.y controller Kp", .01, 0, 1) gen.add("linear_y_Ki", double_t, 0, "linear.y controller Ki", .01, 0, 1) gen.add("linear_y_Kd", double_t, 0, "linear.y controller Kd", .01, 0, 1) diff --git a/workspace/src/detect_targets/scripts/triangle_control.py b/workspace/src/detect_targets/scripts/triangle_control.py index 68e9a96..9c8650c 100755 --- a/workspace/src/detect_targets/scripts/triangle_control.py +++ b/workspace/src/detect_targets/scripts/triangle_control.py @@ -2,6 +2,7 @@ # -*- coding: utf-8 -*- import math +import time import roslib import rospy @@ -51,21 +52,38 @@ class TriangleControl: self.pid_linear_y.Ki = config['linear_y_Ki'] self.pid_linear_y.Kd = config['linear_y_Kd'] self.pid_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0) + self.pid_speed_linear_y.Kp = config['speed_linear_y_Kp'] + self.pid_speed_linear_y.Ki = config['speed_linear_y_Ki'] + self.pid_speed_linear_y.Kd = config['speed_linear_y_Kd'] + self.pid_speed_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0) if not config['control_linear_y']: self.pid_linear_y._last_output = 0.0 + self.pid_speed_linear_y._last_output = 0.0 self.pid_linear_y.output_limits = ( + -config['max_acceleration'], + config['max_acceleration'] + ) + self.pid_speed_linear_y.output_limits = ( -config['max_speed'], config['max_speed'] ) - # X gains are reversed because of the chosen axis self.pid_linear_x.Kp = - config['linear_x_Kp'] self.pid_linear_x.Ki = - config['linear_x_Ki'] self.pid_linear_x.Kd = - config['linear_x_Kd'] self.pid_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0) + self.pid_speed_linear_x.Kp = config['speed_linear_x_Kp'] + self.pid_speed_linear_x.Ki = config['speed_linear_x_Ki'] + self.pid_speed_linear_x.Kd = config['speed_linear_x_Kd'] + self.pid_speed_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0) if not config['control_linear_x']: self.pid_linear_x._last_output = 0.0 + self.pid_speed_linear_x._last_output = 0.0 self.pid_linear_x.output_limits = ( + -config['max_acceleration'], + config['max_acceleration'] + ) + self.pid_speed_linear_x.output_limits = ( -config['max_speed'], config['max_speed'] ) @@ -150,7 +168,12 @@ class TriangleControl: self.linear_z_info.cmd_vel = self.twist.linear.z self.linear_z_pub.publish(self.linear_z_info) - self.twist.linear.y = self.pid_linear_y(-self.alpha) + dt = time.time() - self.pid_linear_y._last_time + last_input_y = self.pid_linear_y._last_input + target_acceleration_y = self.pid_linear_y(-self.alpha) + self.pid_speed_linear_y.setpoint = target_acceleration_y + speed_y = (last_input_y - self.pid_linear_y._last_input) / dt + self.twist.linear.y = self.pid_speed_linear_y(speed_y) if self.linear_y_pub.get_num_connections() > 0: self.linear_y_info.target = 0 self.linear_y_info.error = -self.alpha @@ -158,7 +181,12 @@ class TriangleControl: self.linear_y_info.cmd_vel = self.twist.linear.y self.linear_y_pub.publish(self.linear_y_info) - self.twist.linear.x = self.pid_linear_x(self.d) + dt = time.time() - self.pid_linear_x._last_time + last_input_x = self.pid_linear_x._last_input + target_acceleration_x = self.pid_linear_x(self.d) + self.pid_speed_linear_x.setpoint = target_acceleration_x + speed_x = (last_input_x - self.pid_linear_x._last_input) / dt + self.twist.linear.x = self.pid_speed_linear_x(speed_x) if self.linear_x_pub.get_num_connections() > 0: self.linear_x_info.target = self.pid_linear_x.setpoint self.linear_x_info.error = self.target_distance - self.d @@ -208,6 +236,13 @@ class TriangleControl: auto_mode=True, sample_time=0.14 ) + self.pid_speed_linear_y = PID( + 1, + 0, + 0, + auto_mode=True, + sample_time=0.14 + ) self.pid_linear_x = PID( 1, 0, @@ -216,6 +251,13 @@ class TriangleControl: sample_time=0.14, setpoint=self.target_distance, ) + self.pid_speed_linear_x = PID( + 1, + 0, + 0, + auto_mode=True, + sample_time=0.14 + ) # Control info self.angular_z_info = control()