klafyvel/drone-rigide
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Double PID

Browse source
This commit is contained in:
Hugo LEVY-FALK 2019-03-22 16:59:55 +01:00
parent f5d3507ecf
commit 90d24f4d8f
6 changed files with 233 additions and 44 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

98
workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h
View file

@ -241,10 +241,17 @@ class DEFAULT
if("target_depth"==(*_i)->name){target_depth = boost::any_cast<double>(val);}
if("distance_to_target"==(*_i)->name){distance_to_target = boost::any_cast<double>(val);}
if("max_speed"==(*_i)->name){max_speed = boost::any_cast<double>(val);}
if("max_acceleration"==(*_i)->name){max_acceleration = boost::any_cast<double>(val);}
if("speed_linear_x_Kp"==(*_i)->name){speed_linear_x_Kp = boost::any_cast<double>(val);}
if("speed_linear_x_Ki"==(*_i)->name){speed_linear_x_Ki = boost::any_cast<double>(val);}
if("speed_linear_x_Kd"==(*_i)->name){speed_linear_x_Kd = boost::any_cast<double>(val);}
if("linear_x_Kp"==(*_i)->name){linear_x_Kp = boost::any_cast<double>(val);}
if("linear_x_Ki"==(*_i)->name){linear_x_Ki = boost::any_cast<double>(val);}
if("linear_x_Kd"==(*_i)->name){linear_x_Kd = boost::any_cast<double>(val);}
if("control_linear_x"==(*_i)->name){control_linear_x = boost::any_cast<bool>(val);}
if("speed_linear_y_Kp"==(*_i)->name){speed_linear_y_Kp = boost::any_cast<double>(val);}
if("speed_linear_y_Ki"==(*_i)->name){speed_linear_y_Ki = boost::any_cast<double>(val);}
if("speed_linear_y_Kd"==(*_i)->name){speed_linear_y_Kd = boost::any_cast<double>(val);}
if("linear_y_Kp"==(*_i)->name){linear_y_Kp = boost::any_cast<double>(val);}
if("linear_y_Ki"==(*_i)->name){linear_y_Ki = boost::any_cast<double>(val);}
if("linear_y_Kd"==(*_i)->name){linear_y_Kd = boost::any_cast<double>(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<TriangleParamConfig::DEFAULT, TrianglePara
Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("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<double>("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<double>("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<double>("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<double>("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<double>("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<double>("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<double>("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<double>("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<double>("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<TriangleParamConfig::DEFAULT, TrianglePara
Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<bool>("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<bool>("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<double>("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<double>("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<double>("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<double>("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<double>("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<double>("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"

7
workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox
View file

@ -6,10 +6,17 @@
<param name="target_depth" type="double" value="0.2" />
<param name="distance_to_target" type="double" value="2.0" />
<param name="max_speed" type="double" value="0.3" />
<param name="max_acceleration" type="double" value="0.3" />
<param name="speed_linear_x_Kp" type="double" value="0.01" />
<param name="speed_linear_x_Ki" type="double" value="0.01" />
<param name="speed_linear_x_Kd" type="double" value="0.01" />
<param name="linear_x_Kp" type="double" value="0.01" />
<param name="linear_x_Ki" type="double" value="0.01" />
<param name="linear_x_Kd" type="double" value="0.01" />
<param name="control_linear_x" type="bool" value="True" />
<param name="speed_linear_y_Kp" type="double" value="0.01" />
<param name="speed_linear_y_Ki" type="double" value="0.01" />
<param name="speed_linear_y_Kd" type="double" value="0.01" />
<param name="linear_y_Kp" type="double" value="0.01" />
<param name="linear_y_Ki" type="double" value="0.01" />
<param name="linear_y_Kd" type="double" value="0.01" />

7
workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox
View file

@ -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

110
workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc
View file

@ -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.

7
workspace/src/detect_targets/cfg/triangle_control.cfg
View file

@ -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)

48
workspace/src/detect_targets/scripts/triangle_control.py
View file

@ -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()