diff --git a/include/elevated_control/interface_arm.hpp b/include/elevated_control/interface_arm.hpp index f6eba18..2c11598 100644 --- a/include/elevated_control/interface_arm.hpp +++ b/include/elevated_control/interface_arm.hpp @@ -34,7 +34,12 @@ class ArmInterface : public SynapticonBase { public: struct Config : SynapticonBaseConfig { std::string elevate_config_yaml = "/home/elevate/Desktop/elevate_config.yaml"; - Config() { expected_slave_count = kNumJoints; } + Config() { + // Bus layout: positions 1..kNumJoints are SOMANET joint drives, and the + // final slave is the user-interface device (not a joint). + expected_slave_count = kNumJoints + 1; + num_joints = kNumJoints; + } }; explicit ArmInterface(const Config& config); diff --git a/include/elevated_control/interface_base.hpp b/include/elevated_control/interface_base.hpp index f830aec..f45caad 100644 --- a/include/elevated_control/interface_base.hpp +++ b/include/elevated_control/interface_base.hpp @@ -40,6 +40,11 @@ struct InSomanetSnapshot { struct SynapticonBaseConfig { std::string network_interface = "eno0"; std::size_t expected_slave_count = 0; // 0 = auto-detect, >0 = verify + // Number of joint slaves on the bus. Joints are assumed to occupy EtherCAT + // positions 1..num_joints; any additional trailing slaves (e.g. a + // user-interface device) are left alone by the driver. 0 = use total slave + // count (every detected slave is treated as a joint). + std::size_t num_joints = 0; std::string joint_limits_yaml; }; diff --git a/src/interface_base.cpp b/src/interface_base.cpp index 94601d3..d6fdb8e 100644 --- a/src/interface_base.cpp +++ b/src/interface_base.cpp @@ -155,14 +155,24 @@ std::expected SynapticonBase::Initialize() { Error{ErrorCode::kEtherCATError, "No EtherCAT slaves found"}); } - num_joints_ = static_cast(ec_slavecount); + const std::size_t total_slaves = static_cast(ec_slavecount); if (base_config_.expected_slave_count > 0 && - num_joints_ != base_config_.expected_slave_count) { + total_slaves != base_config_.expected_slave_count) { ec_close(); return std::unexpected(Error{ ErrorCode::kEtherCATError, "Expected " + std::to_string(base_config_.expected_slave_count) + - " slaves but found " + std::to_string(num_joints_)}); + " slaves but found " + std::to_string(total_slaves)}); + } + num_joints_ = base_config_.num_joints > 0 ? base_config_.num_joints + : total_slaves; + if (num_joints_ > total_slaves) { + ec_close(); + return std::unexpected(Error{ + ErrorCode::kEtherCATError, + "Configured num_joints (" + std::to_string(num_joints_) + + ") exceeds detected slave count (" + std::to_string(total_slaves) + + ")"}); } // Resize all per-joint containers @@ -335,7 +345,14 @@ std::expected SynapticonBase::Initialize() { }); initialized_ = true; - spdlog::info("SynapticonBase initialized with {} joints", num_joints_); + if (total_slaves == num_joints_) { + spdlog::info("SynapticonBase initialized with {} joints", num_joints_); + } else { + spdlog::info( + "SynapticonBase initialized with {} joints ({} total EtherCAT slaves, " + "{} trailing non-joint slave(s))", + num_joints_, total_slaves, total_slaves - num_joints_); + } return {}; }