Skip to content
This repository has been archived by the owner on Sep 23, 2024. It is now read-only.

Commit

Permalink
Always be correcting yaw angle, even during starting and stopping.
Browse files Browse the repository at this point in the history
  • Loading branch information
@aentinger
aentinger committed May 14, 2024
1 parent a1e086d commit 0e86522
Show file tree
Hide file tree
Showing 2 changed files with 56 additions and 55 deletions.
3 changes: 2 additions & 1 deletion include/robotem_rovne/Node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,8 @@ class Node : public rclcpp::Node

enum class State { Stopped, Starting, Driving, Stopping };
State _robot_state;
quantity<m/s> _motor_left_vel, _motor_right_vel;
quantity<m/s> _motor_base_vel;
void control_yaw();
void handle_Stopped();
void handle_Starting();
void handle_Driving();
Expand Down
108 changes: 54 additions & 54 deletions src/Node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,7 @@ Node::Node()
, _motor_left_qos_profile{rclcpp::KeepLast(1), rmw_qos_profile_sensor_data}
, _motor_right_qos_profile{rclcpp::KeepLast(1), rmw_qos_profile_sensor_data}
, _robot_state{State::Stopped}
, _motor_left_vel{0. * m/s}
, _motor_right_vel{0. * m/s}
, _motor_base_vel{0. * m/s}
{
init_req_start_service_server();
init_req_stop_service_server();
Expand Down Expand Up @@ -203,85 +202,86 @@ void Node::ctrl_loop()
}
}

void Node::control_yaw()
{
auto const yaw_err = (_yaw_target - _yaw_actual);

double const k = 0.01;
double const pid_res = k * yaw_err.numerical_value_in(deg);

auto const motor_vel_lower_limit = _motor_base_vel - 0.2 * m/s;
auto const motor_vel_upper_limit = _motor_base_vel + 0.2 * m/s;

auto motor_left_vel = _motor_base_vel - pid_res * m/s;
motor_left_vel = std::max(motor_left_vel, motor_vel_lower_limit);
motor_left_vel = std::min(motor_left_vel, motor_vel_upper_limit);

auto motor_right_vel = _motor_base_vel + pid_res * m/s;
motor_right_vel = std::max(motor_right_vel, motor_vel_lower_limit);
motor_right_vel = std::min(motor_right_vel, motor_vel_upper_limit);

RCLCPP_INFO(get_logger(),
"actual = %0.2f, target = %0.2f, error = %0.2f, pid_res = %0.2f, LEFT = %0.2f m/s, RIGHT = %0.2f m/s",
_yaw_actual.numerical_value_in(deg),
_yaw_target.numerical_value_in(deg),
yaw_err.numerical_value_in(deg),
pid_res,
motor_left_vel.numerical_value_in(m/s),
motor_right_vel.numerical_value_in(m/s));

pub_motor_left (motor_left_vel);
pub_motor_right(motor_right_vel);
}

void Node::handle_Stopped()
{
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "handle_Stopped");
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Stopped");

_motor_left_vel = 0. * m/s;
_motor_right_vel = 0. * m/s;
_motor_base_vel = 0. * m/s;

pub_motor_left (_motor_left_vel);
pub_motor_right(_motor_right_vel);
pub_motor_left (0. * m/s);
pub_motor_right(0. * m/s);
}

void Node::handle_Starting()
{
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "handle_Starting");
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Starting");

if (_motor_left_vel < 0.5 * m/s)
_motor_left_vel += 0.01 * m/s;
if (_motor_right_vel < 0.5 * m/s)
_motor_right_vel += 0.01 * m/s;
control_yaw();

if (_motor_right_vel >= 0.5 * m/s && _motor_right_vel >= 0.5 * m/s)
if (_motor_base_vel < 0.5 * m/s)
{
_motor_base_vel += 0.01 * m/s;
}
else
{
_motor_left_vel = 0.5 * m/s;
_motor_right_vel = 0.5 * m/s;
_motor_base_vel = 0.5 * m/s;
_robot_state = State::Driving;
}

pub_motor_left (_motor_left_vel);
pub_motor_right(_motor_right_vel);
}

void Node::handle_Driving()
{
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "handle_Driving");

auto const yaw_err = (_yaw_target - _yaw_actual);

double const k = 0.01;
double const pid_res = k * yaw_err.numerical_value_in(deg);

_motor_left_vel = 0.5 * m/s - pid_res * m/s;
_motor_left_vel = std::max(_motor_left_vel, 0.3 * m/s);
_motor_left_vel = std::min(_motor_left_vel, 0.7 * m/s);
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Driving");

_motor_right_vel = 0.5 * m/s + pid_res * m/s;
_motor_right_vel = std::max(_motor_right_vel, 0.3 * m/s);
_motor_right_vel = std::min(_motor_right_vel, 0.7 * m/s);

RCLCPP_INFO(get_logger(),
"actual = %0.2f, target = %0.2f, error = %0.2f, pid_res = %0.2f, LEFT = %0.2f m/s, RIGHT = %0.2f m/s",
_yaw_actual.numerical_value_in(deg),
_yaw_target.numerical_value_in(deg),
yaw_err.numerical_value_in(deg),
pid_res,
_motor_left_vel.numerical_value_in(m/s),
_motor_right_vel.numerical_value_in(m/s));

pub_motor_left (_motor_left_vel);
pub_motor_right(_motor_right_vel);
control_yaw();
}

void Node::handle_Stopping()
{
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "handle_Stopping");
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Stopping");

if (_motor_left_vel > 0. * m/s)
_motor_left_vel -= 0.025 * m/s;
if (_motor_right_vel > 0. * m/s)
_motor_right_vel -= 0.025 * m/s;
control_yaw();

if (_motor_right_vel <= 0. * m/s && _motor_right_vel <= 0. * m/s)
if (_motor_base_vel > 0. * m/s)
{
_motor_base_vel -= 0.025 * m/s;
}
else
{
_motor_left_vel = 0. * m/s;
_motor_right_vel = 0. * m/s;
_motor_base_vel = 0. * m/s;
_robot_state = State::Stopped;
}

pub_motor_left (_motor_left_vel);
pub_motor_right(_motor_right_vel);
}

/**************************************************************************************
Expand Down

0 comments on commit 0e86522

Please sign in to comment.