From 0fa88cf63a78a55b1331808a9dfab794a380aec1 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 22 Nov 2023 15:14:39 +0100 Subject: [PATCH 1/3] Add simple sequence homing emulation Signed-off-by: Christoph Hellmann Santos --- .../canopen_fake_slaves/cia402_slave.hpp | 54 ++++++++++++++++++- 1 file changed, 53 insertions(+), 1 deletion(-) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp index 4ae7bb26..609152cc 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp @@ -69,6 +69,11 @@ class CIA402MockSlave : public canopen::BasicSlave RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); interpolated_position_mode.join(); } + if (homing_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + homing_mode.join(); + } } protected: @@ -156,6 +161,7 @@ class CIA402MockSlave : public canopen::BasicSlave std::thread cyclic_position_mode; std::thread cyclic_velocity_mode; std::thread interpolated_position_mode; + std::thread homing_mode; double cycle_time; @@ -328,7 +334,42 @@ class CIA402MockSlave : public canopen::BasicSlave } } - void run_position_mode() {} + void run_homing_mode() + { + bool homed = false; + while ((state.load() == InternalState::Operation_Enable) && (operation_mode.load() == Homing) && + (rclcpp::ok())) + { + bool start_homing = control_word & CW_Operation_mode_specific0; + + if (start_homing && !homed) + { + set_status_bit(SW_Manufacturer_specific1); // Motor active + } + else + { + homed = false; + continue; + } + { + std::lock_guard lock(w_mutex); + (*this)[0x6041][0] = status_word; + this->TpdoEvent(1); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + if (start_homing) + { + clear_status_bit(SW_Manufacturer_specific1); // Motor inactive + set_status_bit(SW_Target_reached); // Homing complete + set_status_bit(SW_Operation_mode_specific0); // Homing attained + (*this)[0x6041][0] = status_word; + (*this)[0x6064][0] = static_cast(0); + (*this)[0x606C][0] = static_cast(0); + homed = true; + } + } + } void set_new_status_word_and_state() { @@ -518,6 +559,12 @@ class CIA402MockSlave : public canopen::BasicSlave rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); interpolated_position_mode.join(); } + if (homing_mode.joinable()) + { + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + homing_mode.join(); + } old_operation_mode.store(operation_mode.load()); switch (operation_mode.load()) { @@ -553,6 +600,11 @@ class CIA402MockSlave : public canopen::BasicSlave std::thread(std::bind(&CIA402MockSlave::run_interpolated_position_mode, this)); } + void start_homing_mode() + { + homing_mode = std::thread(std::bind(&CIA402MockSlave::run_homing_mode, this)); + } + void on_quickstop_active() { if (is_enable_operation()) From b19612384399995c62285b2827cfe363dc48ec43 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 22 Nov 2023 15:47:02 +0100 Subject: [PATCH 2/3] Add fake velocity mode Signed-off-by: Christoph Hellmann Santos --- .../canopen_fake_slaves/cia402_slave.hpp | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp index 609152cc..8c7f13c3 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp @@ -74,6 +74,11 @@ class CIA402MockSlave : public canopen::BasicSlave RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); homing_mode.join(); } + if (profiled_velocity_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + profiled_velocity_mode.join(); + } } protected: @@ -334,6 +339,32 @@ class CIA402MockSlave : public canopen::BasicSlave } } + void run_profile_velocity_mode() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_profile_velocity_mode"); + double actual_position = static_cast(((int32_t)(*this)[0x6064][0])) / 1000.0; + double target_velocity = static_cast(((int32_t)(*this)[0x60FF][0])) / 1000.0; + double old_target = target_velocity; + double control_cycle_period_d = 0.01; + while ((state.load() == InternalState::Operation_Enable) && + (operation_mode.load() == Profiled_Velocity) && (rclcpp::ok())) + { + actual_position = static_cast(((int32_t)(*this)[0x6064][0])) / 1000.0; + target_velocity = static_cast(((int32_t)(*this)[0x60FF][0])) / 1000.0; + if (old_target != target_velocity) + { + old_target = target_velocity; + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "New target velocity: %f", target_velocity); + } + (*this)[0x606C][0] = (int32_t)(target_velocity * 1000); + (*this)[0x6064][0] = + (int32_t)((actual_position + target_velocity * control_cycle_period_d) * 1000.0); + std::this_thread::sleep_for( + std::chrono::milliseconds(((int32_t)control_cycle_period_d * 1000))); + } + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Leaving run_profile_velocity_mode"); + } + void run_homing_mode() { bool homed = false; @@ -565,6 +596,12 @@ class CIA402MockSlave : public canopen::BasicSlave rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); homing_mode.join(); } + if (profiled_velocity_mode.joinable()) + { + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + profiled_velocity_mode.join(); + } old_operation_mode.store(operation_mode.load()); switch (operation_mode.load()) { @@ -577,6 +614,12 @@ class CIA402MockSlave : public canopen::BasicSlave case Interpolated_Position: start_interpolated_pos_mode(); break; + case Homing: + start_homing_mode(); + break; + case Profiled_Velocity: + start_profile_velocity_mode(); + break; default: break; } @@ -605,6 +648,12 @@ class CIA402MockSlave : public canopen::BasicSlave homing_mode = std::thread(std::bind(&CIA402MockSlave::run_homing_mode, this)); } + void start_profile_velocity_mode() + { + profiled_velocity_mode = + std::thread(std::bind(&CIA402MockSlave::run_profile_velocity_mode, this)); + } + void on_quickstop_active() { if (is_enable_operation()) From 0aaca9afcef6ff48a5c0c19725a5cdbe9d228343 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 22 Nov 2023 16:52:46 +0100 Subject: [PATCH 3/3] Formatting Signed-off-by: Christoph Hellmann Santos --- .../include/canopen_fake_slaves/cia402_slave.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp index 8a9f0f8a..8c7f13c3 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp @@ -364,7 +364,7 @@ class CIA402MockSlave : public canopen::BasicSlave } RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Leaving run_profile_velocity_mode"); } - + void run_homing_mode() { bool homed = false; @@ -648,13 +648,12 @@ class CIA402MockSlave : public canopen::BasicSlave homing_mode = std::thread(std::bind(&CIA402MockSlave::run_homing_mode, this)); } - void start_profile_velocity_mode() { profiled_velocity_mode = std::thread(std::bind(&CIA402MockSlave::run_profile_velocity_mode, this)); } - + void on_quickstop_active() { if (is_enable_operation())