Skip to content
This repository has been archived by the owner on Dec 1, 2020. It is now read-only.

Fix return to stand when close is finished. #39

Merged
merged 3 commits into from
Jan 17, 2020
Merged
Changes from all commits
Commits
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
7 changes: 6 additions & 1 deletion march_gazebo_plugins/src/com_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ class ComController : public ModelPlugin
this->p_pitch = 60;
this->d_pitch = 10;
this->p_roll = 150;
this->d_roll = 7.5;
this->d_roll = 0;
this->p_yaw = 500;
this->d_yaw = 25;

Expand Down Expand Up @@ -107,6 +107,11 @@ class ComController : public ModelPlugin
goal_position_x += 0.25 * swing_step_size - 0.25 * time_since_start * swing_step_size / subgait_duration;
}

if (time_since_start > subgait_duration)
{
this->subgait_name = "home_stand";
}

double error_x = model_com.X() - goal_position_x;
double error_y = model_com.Y() - goal_position_y;
double error_yaw = this->foot_left->WorldPose().Rot().Z();
Expand Down