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

Add fake profile velocity #230

Merged
merged 4 commits into from
Nov 22, 2023
Merged

Add fake profile velocity #230

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
0fa88cf
Add simple sequence homing emulation
Nov 22, 2023
b196123
Add fake velocity mode
Nov 22, 2023
5567517
Merge branch 'master' into add-fake-profile-velocity
hellantos Nov 22, 2023
0aaca9a
Formatting
Nov 22, 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
49 changes: 49 additions & 0 deletions canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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<double>(((int32_t)(*this)[0x6064][0])) / 1000.0;
double target_velocity = static_cast<double>(((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<double>(((int32_t)(*this)[0x6064][0])) / 1000.0;
target_velocity = static_cast<double>(((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;
Expand Down Expand Up @@ -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())
{
Expand All @@ -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;
}
Expand Down Expand Up @@ -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())
Expand Down
Loading