Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Allow different PID values for pre/post rotate and target vs max error #3

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Allow different PID values for pre/post rotate and target vs max error #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions cfg/FTCPlanner.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,12 +25,18 @@ grp_pid.add("kp_ang", double_t, 0, "KP for angle controller", 1.0, 0.0, 50.0)
grp_pid.add("ki_ang", double_t, 0, "KI for angle controller", 0.0, 0.0, 5.0)
grp_pid.add("ki_ang_max", double_t, 0, "KI windup for speed controller angle", 10.0, 0.0, 500.0)
grp_pid.add("kd_ang", double_t, 0, "KD for angle controller", 0.0, 0.0, 5.0)
grp_pid.add("kp_ang_rotate", double_t, 0, "KP for angle in pre/post rotate", 1.0, 0.0, 50.0)
grp_pid.add("ki_ang_rotate", double_t, 0, "KI for angle in pre/post rotate", 0.0, 0.0, 5.0)
grp_pid.add("ki_ang_max_rotate", double_t, 0, "KI windup for angle in pre/post rotate", 10.0, 0.0, 500.0)
grp_pid.add("kd_ang_rotate", double_t, 0, "KD for angle in pre/post rotate", 0.0, 0.0, 5.0)
Comment on lines +28 to +31
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is it intended that these values are the same as those without _rotate? So different settings would be possible, but not used by default?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Exactly, didn't want to change current behaviour but I'm using higher values

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Which values do you use then? I'll see if the target vs. error helps, but if not, it would be good to know which values to increase how much.

Copy link
Author

@olliewalsh olliewalsh May 20, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm using completely different values as I've changed how the motors are controlled.
Looking at https://github.com/ClemensElflein/open_mower_ros/blob/main/src/open_mower/params/ftc_local_planner.yaml
I would start with kp_ang_rotate: 3.5, ki_ang_rotate: 0.5


grp_bot = gen.add_group("Robot", type="tab")
grp_bot.add("max_cmd_vel_speed", double_t, 0, "Max speed to send to the controller", 2.0, 0.0, 5.0)
grp_bot.add("max_cmd_vel_ang", double_t, 0, "Max angular speed to send to the controller", 2.0, 0.0, 5.0)
grp_bot.add("max_goal_distance_error", double_t, 0, "Wait for robot to get closer than max_goal_distance_error to the goal before setting goal as finished", 1.0, 0.0, 3.0)
grp_bot.add("max_goal_angle_error", double_t, 0, "Wait for robot to get a better angle than max_goal_angle_error before setting goal as finished", 10.0, 0.0, 360.0)
grp_bot.add("max_goal_distance_error", double_t, 0, "If robot does not get closer than max_goal_distance_error to the goal by goal_timeout assume crashed", 1.0, 0.0, 3.0)
grp_bot.add("target_goal_distance_error", double_t, 0, "Wait for robot to get closer than target_goal_distance_error to the goal before setting goal as finished", 1.0, 0.0, 3.0)
grp_bot.add("max_goal_angle_error", double_t, 0, "If robot does not get a better angle than max_goal_angle_error by goal_timeout assume crashed", 10.0, 0.0, 360.0)
grp_bot.add("target_goal_angle_error", double_t, 0, "Wait for robot to get a better angle than target_goal_angle_error before setting goal as finished", 10.0, 0.0, 360.0)
grp_bot.add("goal_timeout", double_t, 0, "Timeout for max_goal_distance_error and max_goal_angle_error", 5.0, 0.0, 100.0)
grp_bot.add("max_follow_distance", double_t, 0, "Stop controller if robot is further away than max_follow_distance (i.e. crash detection)", 1.0, 0.0, 3.0)

Expand Down
30 changes: 21 additions & 9 deletions src/ftc_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -269,12 +269,12 @@ namespace ftc_local_planner
break;
case POST_ROTATE:
{
if (time_in_current_state() > config.goal_timeout)
if (time_in_current_state() > config.goal_timeout && abs(angle_error) * (180.0 / M_PI) > config.max_goal_angle_error)
{
ROS_WARN_STREAM("FTCLocalPlannerROS: Could not reach goal rotation. config.goal_timeout (" << config.goal_timeout << ") reached");
return FINISHED;
}
if (abs(angle_error) * (180.0 / M_PI) < config.max_goal_angle_error)
if (abs(angle_error) * (180.0 / M_PI) <= config.target_goal_angle_error)
{
ROS_INFO_STREAM("FTCLocalPlannerROS: POST_ROTATE finished.");
return FINISHED;
Expand Down Expand Up @@ -451,13 +451,25 @@ namespace ftc_local_planner
{
i_lat_error = -config.ki_lat_max;
}
if (i_angle_error > config.ki_ang_max)
{
i_angle_error = config.ki_ang_max;
if (current_state == FOLLOWING) {
if (i_angle_error > config.ki_ang_max)
{
i_angle_error = config.ki_ang_max;
}
else if (i_angle_error < -config.ki_ang_max)
{
i_angle_error = -config.ki_ang_max;
}
}
else if (i_angle_error < -config.ki_ang_max)
{
i_angle_error = -config.ki_ang_max;
else {
if (i_angle_error > config.ki_ang_max_rotate)
{
i_angle_error = config.ki_ang_max_rotate;
}
else if (i_angle_error < -config.ki_ang_max_rotate)
{
i_angle_error = -config.ki_ang_max_rotate;
}
}

double d_lat = (lat_error - last_lat_error) / dt;
Expand Down Expand Up @@ -519,7 +531,7 @@ namespace ftc_local_planner
}
else
{
double ang_speed = angle_error * config.kp_ang + i_angle_error * config.ki_ang + d_angle * config.kd_ang;
double ang_speed = angle_error * config.kp_ang_rotate + i_angle_error * config.ki_ang_rotate + d_angle * config.kd_ang_rotate;
if (ang_speed > config.max_cmd_vel_ang)
{
ang_speed = config.max_cmd_vel_ang;
Expand Down