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.

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

add configurable homing timeout #442

Open
wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from
Open
Show file tree
Hide file tree
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
6 changes: 5 additions & 1 deletion canopen_402/include/canopen_402/motor.h
Original file line number Diff line number Diff line change
Expand Up @@ -261,6 +261,7 @@ class HomingMode: public Mode{
public:
HomingMode() : Mode(MotorBase::Homing) {}
virtual bool executeHoming(canopen::LayerStatus &status) = 0;
virtual bool executeHoming(canopen::LayerStatus &status, const boost::chrono::seconds &homing_timeout) = 0;
};

class DefaultHomingMode: public HomingMode{
Expand All @@ -286,6 +287,7 @@ class DefaultHomingMode: public HomingMode{
virtual bool write(OpModeAccesser& cw);

virtual bool executeHoming(canopen::LayerStatus &status);
virtual bool executeHoming(canopen::LayerStatus &status, const boost::chrono::seconds &homing_timeout);
};

class Motor402 : public MotorBase
Expand All @@ -296,7 +298,8 @@ class Motor402 : public MotorBase
: MotorBase(name), status_word_(0),control_word_(0),
switching_state_(State402::InternalState(settings.get_optional<unsigned int>("switching_state", static_cast<unsigned int>(State402::Operation_Enable)))),
monitor_mode_(settings.get_optional<bool>("monitor_mode", true)),
state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5))
state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5)),
homing_timeout_(settings.get_optional<unsigned int>("homing_timeout", 10))
{
storage->entry(status_word_entry_, 0x6041);
storage->entry(control_word_entry_, 0x6040);
Expand Down Expand Up @@ -377,6 +380,7 @@ class Motor402 : public MotorBase
const State402::InternalState switching_state_;
const bool monitor_mode_;
const boost::chrono::seconds state_switch_timeout_;
const boost::chrono::seconds homing_timeout_;

canopen::ObjectStorage::Entry<uint16_t> status_word_entry_;
canopen::ObjectStorage::Entry<uint16_t > control_word_entry_;
Expand Down
57 changes: 56 additions & 1 deletion canopen_402/src/motor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -242,6 +242,61 @@ bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) {
return error(status, "something went wrong while homing");
}

bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status, const boost::chrono::seconds &homing_timeout) {
if(!homing_method_.valid()){
return error(status, "homing method entry is not valid");
}

if(homing_method_.get_cached() == 0){
return true;
}

time_point prepare_time = get_abs_time(boost::chrono::seconds(1));
// ensure homing is not running
boost::mutex::scoped_lock lock(mutex_);
if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
return error(status, "could not prepare homing");
}
if(status_ & MASK_Error){
return error(status, "homing error before start");
}

execute_ = true;

// ensure start
if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Attained | MASK_Reached, MASK_Reached> (status_))){
return error(status, "homing did not start");
}
if(status_ & MASK_Error){
return error(status, "homing error at start");
}

time_point finish_time = get_abs_time(homing_timeout); //

// wait for attained
if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Attained, 0> (status_))){
return error(status, "homing not attained");
}
if(status_ & MASK_Error){
return error(status, "homing error during process");
}

// wait for motion stop
if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
return error(status, "homing did not stop");
}
if(status_ & MASK_Error){
return error(status, "homing error during stop");
}

if((status_ & MASK_Reached) && (status_ & MASK_Attained)){
execute_ = false;
return true;
}

return error(status, "something went wrong while homing");
}

bool Motor402::setTarget(double val){
if(state_handler_.getState() == State402::Operation_Enable){
boost::mutex::scoped_lock lock(mode_mutex_);
Expand Down Expand Up @@ -504,7 +559,7 @@ void Motor402::handleInit(LayerStatus &status){
return;
}

if(!homing->executeHoming(status)){
if(!homing->executeHoming(status, homing_timeout_)){
status.error("Homing failed");
return;
}
Expand Down