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

Homing timeout #250

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Homing timeout #250

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
dcc514d
Attempt to make homing timeout a parameter in seconds
zlizer Dec 19, 2023
8d5cbf9
Don't overwrite value, print on read
Dec 20, 2023
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
4 changes: 3 additions & 1 deletion canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@ class DefaultHomingMode : public HomingMode
std::condition_variable cond_;
uint16_t status_;

const int homing_timeout_seconds_;

enum SW_masks
{
MASK_Reached = (1 << State402::SW_Target_reached),
Expand All @@ -49,7 +51,7 @@ class DefaultHomingMode : public HomingMode
}

public:
DefaultHomingMode(std::shared_ptr<LelyDriverBridge> driver) { this->driver = driver; }
DefaultHomingMode(std::shared_ptr<LelyDriverBridge> driver, int homing_timeout_seconds) : homing_timeout_seconds_(homing_timeout_seconds) { this->driver = driver; }
virtual bool start();
virtual bool read(const uint16_t & sw);
virtual bool write(OpModeAccesser & cw);
Expand Down
7 changes: 4 additions & 3 deletions canopen_402_driver/include/canopen_402_driver/motor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,8 @@ class Motor402 : public MotorBase
{
public:
Motor402(
std::shared_ptr<LelyDriverBridge> driver, ros2_canopen::State402::InternalState switching_state)
: MotorBase(), switching_state_(switching_state), monitor_mode_(true), state_switch_timeout_(5)
std::shared_ptr<LelyDriverBridge> driver, ros2_canopen::State402::InternalState switching_state, int homing_timeout_seconds)
: MotorBase(), switching_state_(switching_state), monitor_mode_(true), state_switch_timeout_(5), homing_timeout_seconds_(homing_timeout_seconds)
{
this->driver = driver;
}
Expand Down Expand Up @@ -167,7 +167,7 @@ class Motor402 : public MotorBase
registerMode<VelocityMode>(MotorBase::Velocity, driver);
registerMode<ProfiledVelocityMode>(MotorBase::Profiled_Velocity, driver);
registerMode<ProfiledTorqueMode>(MotorBase::Profiled_Torque, driver);
registerMode<DefaultHomingMode>(MotorBase::Homing, driver);
registerMode<DefaultHomingMode>(MotorBase::Homing, driver, homing_timeout_seconds_);
registerMode<InterpolatedPositionMode>(MotorBase::Interpolated_Position, driver);
registerMode<CyclicSynchronousPositionMode>(MotorBase::Cyclic_Synchronous_Position, driver);
registerMode<CyclicSynchronousVelocityMode>(MotorBase::Cyclic_Synchronous_Velocity, driver);
Expand Down Expand Up @@ -215,6 +215,7 @@ class Motor402 : public MotorBase
const State402::InternalState switching_state_;
const bool monitor_mode_;
const std::chrono::seconds state_switch_timeout_;
const int homing_timeout_seconds_;

std::shared_ptr<LelyDriverBridge> driver;
const uint16_t status_word_entry_index = 0x6041;
Expand Down
1 change: 1 addition & 0 deletions canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
double scale_vel_to_dev_;
double scale_vel_from_dev_;
ros2_canopen::State402::InternalState switching_state_;
int homing_timeout_seconds_;

void publish();
virtual void poll_timer_callback() override;
Expand Down
28 changes: 23 additions & 5 deletions ...en_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -184,6 +184,7 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
std::optional<double> scale_vel_to_dev;
std::optional<double> scale_vel_from_dev;
std::optional<int> switching_state;
std::optional<int> homing_timeout_seconds;
try
{
scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].as<double>());
Expand Down Expand Up @@ -219,6 +220,13 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
catch (...)
{
}
try
{
homing_timeout_seconds = std::optional(this->config_["homing_timout_seconds"].as<int>());
}
catch (...)
{
}

// auto period = this->config_["scale_eff_to_dev"].as<double>();
// auto period = this->config_["scale_eff_from_dev"].as<double>();
Expand All @@ -228,10 +236,11 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::configure(bool calle
scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or(
(int)ros2_canopen::State402::InternalState::Operation_Enable);
homing_timeout_seconds_ = homing_timeout_seconds.value_or(10);
RCLCPP_INFO(
this->node_->get_logger(),
"scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ %f\n",
scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_);
"scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ %f\nhoming_timeout_seconds_ %i\n",
scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_, homing_timeout_seconds_);
}

template <>
Expand All @@ -243,6 +252,7 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
std::optional<double> scale_vel_to_dev;
std::optional<double> scale_vel_from_dev;
std::optional<int> switching_state;
std::optional<int> homing_timeout_seconds;
try
{
scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].as<double>());
Expand Down Expand Up @@ -278,6 +288,13 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
catch (...)
{
}
try
{
homing_timeout_seconds = std::optional(this->config_["homing_timeout_seconds"].as<int>());
}
catch (...)
{
}

// auto period = this->config_["scale_eff_to_dev"].as<double>();
// auto period = this->config_["scale_eff_from_dev"].as<double>();
Expand All @@ -287,10 +304,11 @@ void NodeCanopen402Driver<rclcpp::Node>::configure(bool called_from_base)
scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001);
switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or(
(int)ros2_canopen::State402::InternalState::Operation_Enable);
homing_timeout_seconds_ = homing_timeout_seconds.value_or(10);
RCLCPP_INFO(
this->node_->get_logger(),
"scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ %f\n",
scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_);
"scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ %f\nhoming_timeout_seconds_ %i\n",
scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_, homing_timeout_seconds_);
}

template <class NODETYPE>
Expand Down Expand Up @@ -332,7 +350,7 @@ template <class NODETYPE>
void NodeCanopen402Driver<NODETYPE>::add_to_master()
{
NodeCanopenProxyDriver<NODETYPE>::add_to_master();
motor_ = std::make_shared<Motor402>(this->lely_driver_, switching_state_);
motor_ = std::make_shared<Motor402>(this->lely_driver_, switching_state_, homing_timeout_seconds_);
}

template <class NODETYPE>
Expand Down
5 changes: 4 additions & 1 deletion canopen_402_driver/src/default_homing_mode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,8 +87,11 @@ bool DefaultHomingMode::executeHoming()
return error("homing error at start");
}

// std::chrono::steady_clock::time_point finish_time =
// std::chrono::steady_clock::now() + std::chrono::seconds(10); //
//
std::chrono::steady_clock::time_point finish_time =
std::chrono::steady_clock::now() + std::chrono::seconds(10); //
std::chrono::steady_clock::now() + std::chrono::seconds(homing_timeout_seconds_); //

// wait for attained
if (!cond_.wait_until(
Expand Down
Loading