add configurable homing timeout

Signed-off-by: Servando German Serrano <[email protected]>
ros-industrial · Aug 19, 2021 · 3451669 · 3451669
1 parent 4e2dce2
commit 3451669
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/canopen_402/include/canopen_402/motor.h b/canopen_402/include/canopen_402/motor.h
@@ -261,10 +261,12 @@ class HomingMode: public Mode{
 public:
     HomingMode() : Mode(MotorBase::Homing) {}
     virtual bool executeHoming(canopen::LayerStatus &status) = 0;
+    virtual void setHomingTimeout(const boost::chrono::seconds &homing_timeout) = 0;
 };
 
 class DefaultHomingMode: public HomingMode{
     canopen::ObjectStorage::Entry<int8_t> homing_method_;
+    boost::chrono::seconds homing_timeout_;
     std::atomic<bool> execute_;
 
     boost::mutex mutex_;
@@ -286,6 +288,7 @@ class DefaultHomingMode: public HomingMode{
     virtual bool write(OpModeAccesser& cw);
 
     virtual bool executeHoming(canopen::LayerStatus &status);
+    virtual void setHomingTimeout(const boost::chrono::seconds &homing_timeout);
 };
 
 class Motor402 : public MotorBase
@@ -296,7 +299,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);
@@ -377,6 +381,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_;

diff --git a/canopen_402/src/motor.cpp b/canopen_402/src/motor.cpp
@@ -216,7 +216,7 @@ bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) {
         return error(status, "homing error at start");
     }
 
-    time_point finish_time = get_abs_time(boost::chrono::seconds(10)); //
+    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_))){
@@ -242,6 +242,10 @@ bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) {
     return error(status, "something went wrong while homing");
 }
 
+void DefaultHomingMode::setHomingTimeout(const boost::chrono::seconds &homing_timeout) {
+    homing_timeout_ = homing_timeout;
+}
+
 bool Motor402::setTarget(double val){
     if(state_handler_.getState() == State402::Operation_Enable){
         boost::mutex::scoped_lock lock(mode_mutex_);
@@ -504,6 +508,8 @@ void Motor402::handleInit(LayerStatus &status){
         return;
     }
 
+    homing->setHomingTimeout(homing_timeout_);
+
     if(!homing->executeHoming(status)){
         status.error("Homing failed");
         return;