feat(motor): add EYOU EtherCAT CiA402 driver
This commit is contained in:
parent
c9c7e43a70
commit
dc0831eb71
@ -1,6 +1,10 @@
|
||||
add_library(motor_core INTERFACE)
|
||||
|
||||
target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices)
|
||||
target_include_directories(motor_core
|
||||
INTERFACE
|
||||
${CMAKE_SOURCE_DIR}/cmvr-es
|
||||
${CMAKE_SOURCE_DIR}/cmvr-es/devices
|
||||
)
|
||||
|
||||
target_link_libraries(motor_core
|
||||
INTERFACE
|
||||
@ -12,4 +16,5 @@ add_library(cmvr_es::device::motor_core ALIAS motor_core)
|
||||
add_subdirectory(drivers/ti5_canopen)
|
||||
add_subdirectory(drivers/mujoco)
|
||||
add_subdirectory(bus_runtime)
|
||||
add_subdirectory(drivers/ethercat_motor)
|
||||
add_subdirectory(manager)
|
||||
|
||||
49
cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt
Normal file
49
cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt
Normal file
@ -0,0 +1,49 @@
|
||||
add_library(ethercat_motor_driver SHARED
|
||||
src/cia402/cia402_protocol.cpp
|
||||
src/vendor/eyou/eyou_motor.cpp
|
||||
src/vendor/eyou/eyou_motor_adapter.cpp
|
||||
)
|
||||
|
||||
target_include_directories(ethercat_motor_driver
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
)
|
||||
|
||||
target_link_libraries(ethercat_motor_driver
|
||||
PUBLIC
|
||||
cmvr_es::device::motor_core
|
||||
cmvr_es::device::motor_bus_runtime
|
||||
PRIVATE
|
||||
cmvr_es::proto
|
||||
glog
|
||||
)
|
||||
|
||||
add_library(cmvr_es::device::ethercat_motor_driver ALIAS ethercat_motor_driver)
|
||||
install(TARGETS ethercat_motor_driver LIBRARY DESTINATION lib)
|
||||
|
||||
add_executable(eyou_motor_real_test
|
||||
src/vendor/eyou/eyou_motor_real_test.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(eyou_motor_real_test
|
||||
PRIVATE
|
||||
cmvr_es::device::ethercat_motor_driver
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
)
|
||||
|
||||
add_executable(eyou_motor_device_manager_real_test
|
||||
src/vendor/eyou/eyou_motor_device_manager_real_test.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(eyou_motor_device_manager_real_test
|
||||
PRIVATE
|
||||
cmvr_es::device_manager
|
||||
cmvr_es::device::motor_manager
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
)
|
||||
@ -0,0 +1,161 @@
|
||||
#ifndef CMVR_ES_CIA402_OBJECTS_H
|
||||
#define CMVR_ES_CIA402_OBJECTS_H
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace cmvr::device::cia402 {
|
||||
|
||||
union Controlword {
|
||||
std::uint16_t value;
|
||||
struct {
|
||||
std::uint16_t switch_on : 1;
|
||||
std::uint16_t enable_voltage : 1;
|
||||
std::uint16_t quick_stop : 1;
|
||||
std::uint16_t enable_operation : 1;
|
||||
std::uint16_t new_set_point : 1;
|
||||
std::uint16_t change_set_immediately : 1;
|
||||
std::uint16_t relative : 1;
|
||||
std::uint16_t fault_reset : 1;
|
||||
std::uint16_t halt : 1;
|
||||
std::uint16_t reserved : 2;
|
||||
std::uint16_t manufacturer_specific : 5;
|
||||
};
|
||||
};
|
||||
|
||||
union Statusword {
|
||||
std::uint16_t value;
|
||||
struct {
|
||||
std::uint16_t ready_to_switch_on : 1;
|
||||
std::uint16_t switched_on : 1;
|
||||
std::uint16_t operation_enabled : 1;
|
||||
std::uint16_t fault : 1;
|
||||
std::uint16_t voltage_enabled : 1;
|
||||
std::uint16_t quick_stop : 1;
|
||||
std::uint16_t switch_on_disabled : 1;
|
||||
std::uint16_t warning : 1;
|
||||
std::uint16_t manufacturer_specific_8 : 1;
|
||||
std::uint16_t remote : 1;
|
||||
std::uint16_t target_reached : 1;
|
||||
std::uint16_t internal_limit_active : 1;
|
||||
std::uint16_t operation_mode_specific : 2;
|
||||
std::uint16_t manufacturer_specific : 2;
|
||||
};
|
||||
};
|
||||
|
||||
static_assert(sizeof(Controlword) == sizeof(std::uint16_t));
|
||||
static_assert(sizeof(Statusword) == sizeof(std::uint16_t));
|
||||
|
||||
enum class DeviceState {
|
||||
SwitchOnDisabled,
|
||||
ReadyToSwitchOn,
|
||||
SwitchedOn,
|
||||
OperationEnabled,
|
||||
};
|
||||
|
||||
namespace detail {
|
||||
|
||||
struct StateRule {
|
||||
std::uint16_t relevant_bits;
|
||||
std::uint16_t expected_bits;
|
||||
};
|
||||
|
||||
inline StateRule stateRule(const DeviceState state)
|
||||
{
|
||||
// CiA402 device states are matched by selected 0x6041 statusword bits.
|
||||
switch (state) {
|
||||
case DeviceState::SwitchOnDisabled:
|
||||
return {0x004F, 0x0040};
|
||||
case DeviceState::ReadyToSwitchOn:
|
||||
return {0x006F, 0x0021};
|
||||
case DeviceState::SwitchedOn:
|
||||
return {0x006F, 0x0023};
|
||||
case DeviceState::OperationEnabled:
|
||||
return {0x006F, 0x0027};
|
||||
}
|
||||
return {0x006F, 0x0000};
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
inline Controlword controlword(const std::uint16_t value)
|
||||
{
|
||||
Controlword cw{};
|
||||
cw.value = value;
|
||||
return cw;
|
||||
}
|
||||
|
||||
inline Statusword statusword(const std::uint16_t value)
|
||||
{
|
||||
Statusword sw{};
|
||||
sw.value = value;
|
||||
return sw;
|
||||
}
|
||||
|
||||
inline Controlword shutdownControlword()
|
||||
{
|
||||
Controlword cw{};
|
||||
cw.quick_stop = 1;
|
||||
cw.enable_voltage = 1;
|
||||
return cw;
|
||||
}
|
||||
|
||||
inline Controlword switchOnControlword()
|
||||
{
|
||||
auto cw = shutdownControlword();
|
||||
cw.switch_on = 1;
|
||||
return cw;
|
||||
}
|
||||
|
||||
inline Controlword enableOperationControlword()
|
||||
{
|
||||
auto cw = switchOnControlword();
|
||||
cw.enable_operation = 1;
|
||||
return cw;
|
||||
}
|
||||
|
||||
inline Controlword quickStopControlword()
|
||||
{
|
||||
auto cw = enableOperationControlword();
|
||||
cw.quick_stop = 0;
|
||||
return cw;
|
||||
}
|
||||
|
||||
inline Controlword faultResetControlword()
|
||||
{
|
||||
Controlword cw{};
|
||||
cw.fault_reset = 1;
|
||||
return cw;
|
||||
}
|
||||
|
||||
inline Controlword profilePositionControlword(const bool new_set_point)
|
||||
{
|
||||
auto cw = enableOperationControlword();
|
||||
cw.change_set_immediately = 1;
|
||||
cw.new_set_point = new_set_point ? 1 : 0;
|
||||
return cw;
|
||||
}
|
||||
|
||||
inline bool hasState(const Statusword status, const DeviceState state)
|
||||
{
|
||||
const auto rule = detail::stateRule(state);
|
||||
return (status.value & rule.relevant_bits) == rule.expected_bits;
|
||||
}
|
||||
|
||||
inline bool isSwitchOnDisabled(const Statusword status)
|
||||
{
|
||||
return hasState(status, DeviceState::SwitchOnDisabled);
|
||||
}
|
||||
|
||||
inline bool isOperationEnabled(const Statusword status)
|
||||
{
|
||||
return hasState(status, DeviceState::OperationEnabled);
|
||||
}
|
||||
|
||||
inline bool targetReached(const Statusword status)
|
||||
{
|
||||
return status.target_reached != 0;
|
||||
}
|
||||
|
||||
} // namespace cmvr::device::cia402
|
||||
|
||||
#endif // CMVR_ES_CIA402_OBJECTS_H
|
||||
@ -0,0 +1,116 @@
|
||||
#ifndef CMVR_ES_CIA402_PROTOCOL_H
|
||||
#define CMVR_ES_CIA402_PROTOCOL_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "cmvr/config/motor_config/motor_config.pb.h"
|
||||
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
|
||||
#include "devices/motor/motor_protocol_interface.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
class Cia402Protocol final : public MotorProtocolInterface {
|
||||
public:
|
||||
explicit Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
|
||||
const config::Cia402ProtocolConfig& config);
|
||||
~Cia402Protocol() override = default;
|
||||
|
||||
bool initNode(std::uint8_t node_id) override;
|
||||
|
||||
void setMode(std::uint8_t node_id, msgs::RunMode mode) override;
|
||||
msgs::RunMode getMode(std::uint8_t node_id) override;
|
||||
void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override;
|
||||
void setLimitQd(std::uint8_t node_id, double qd) override;
|
||||
void setLimitQ(std::uint8_t node_id, double ub, double lb) override;
|
||||
bool calibrateZeroQ(std::uint8_t node_id) override;
|
||||
bool reachedTargetQ(std::uint8_t node_id) override;
|
||||
bool commandProfilePosition(std::uint8_t node_id,
|
||||
double target_q,
|
||||
double max_qd,
|
||||
double max_qdd) override;
|
||||
bool commandProfileVelocity(std::uint8_t node_id,
|
||||
double target_qd,
|
||||
double max_qdd) override;
|
||||
bool commandCyclicPosition(std::uint8_t node_id,
|
||||
double target_q,
|
||||
double target_qd) override;
|
||||
bool commandCyclicVelocity(std::uint8_t node_id,
|
||||
double target_qd) override;
|
||||
bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override;
|
||||
void setMotorConversion(std::uint8_t node_id,
|
||||
double encoder_counts_per_rev,
|
||||
double gear_ratio) override;
|
||||
bool torqueOn(std::uint8_t node_id) override;
|
||||
bool torqueOff(std::uint8_t node_id) override;
|
||||
bool brakeRelease(std::uint8_t node_id) override;
|
||||
bool quickStop(std::uint8_t node_id) override;
|
||||
|
||||
double getQ(std::uint8_t node_id) override;
|
||||
double getQd(std::uint8_t node_id) override;
|
||||
bool syncTargetToActualPosition(std::uint8_t node_id);
|
||||
|
||||
private:
|
||||
struct NodeState {
|
||||
msgs::RunMode mode{msgs::RUN_MODE_CYCLIC_SYNC_POSITION};
|
||||
cia402::Controlword controlword{};
|
||||
std::int32_t target_position{0};
|
||||
std::int32_t target_velocity{0};
|
||||
std::int16_t target_torque{0};
|
||||
std::int32_t profile_velocity{0};
|
||||
std::int32_t profile_acceleration{0};
|
||||
std::int32_t profile_deceleration{0};
|
||||
double limit_q_lb{0.0};
|
||||
double limit_q_ub{0.0};
|
||||
double limit_qd{0.0};
|
||||
double limit_qdd{0.0};
|
||||
double encoder_counts_per_rev{0.0};
|
||||
double gear_ratio{0.0};
|
||||
};
|
||||
|
||||
static std::int8_t toCia402Mode_(msgs::RunMode mode);
|
||||
static msgs::RunMode fromCia402Mode_(std::int8_t mode);
|
||||
static cia402::Controlword nextControlword_(cia402::Statusword statusword);
|
||||
static bool isOperationEnabled_(cia402::Statusword statusword);
|
||||
static bool targetReached_(cia402::Statusword statusword);
|
||||
|
||||
std::int32_t radToCounts_(double angle_rad, const NodeState& state) const;
|
||||
double countsToRad_(std::int32_t counts, const NodeState& state) const;
|
||||
std::int32_t radPerSecToCounts_(double velocity_rad_s, const NodeState& state) const;
|
||||
std::int32_t radPerSec2ToCounts_(double acceleration_rad_s2, const NodeState& state) const;
|
||||
double countsToRadPerSec_(std::int32_t velocity_counts_s, const NodeState& state) const;
|
||||
NodeState& nodeState_(std::uint8_t node_id);
|
||||
const NodeState* findNodeState_(std::uint8_t node_id) const;
|
||||
bool hasValidConversion_(std::uint8_t node_id, const NodeState& state) const;
|
||||
bool validateNodePdos_(std::uint8_t node_id) const;
|
||||
bool readStatusword_(std::uint8_t node_id, std::uint16_t& statusword) const;
|
||||
bool readActualPosition_(std::uint8_t node_id, std::int32_t& actual_position) const;
|
||||
bool readActualVelocity_(std::uint8_t node_id, std::int32_t& actual_velocity) const;
|
||||
bool readModeDisplay_(std::uint8_t node_id, std::int8_t& mode_display) const;
|
||||
bool writeControlword_(std::uint8_t node_id, cia402::Controlword controlword);
|
||||
bool writeControlwordAndWait_(std::uint8_t node_id,
|
||||
cia402::Controlword controlword,
|
||||
cia402::DeviceState target_state,
|
||||
const char* state_name);
|
||||
bool waitStatus_(std::uint8_t node_id,
|
||||
cia402::DeviceState target_state,
|
||||
const char* state_name) const;
|
||||
bool waitVelocityNearZero_(std::uint8_t node_id, const char* action_name) const;
|
||||
bool writePositionLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
|
||||
bool writeVelocityLimitToDictionary_(std::uint8_t node_id, const NodeState& state) const;
|
||||
bool writeAccelerationLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
|
||||
bool prepareSafeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, NodeState& state);
|
||||
void writeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, const NodeState& state) const;
|
||||
void writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state);
|
||||
void writeNode_(std::uint8_t node_id, NodeState& state);
|
||||
|
||||
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
|
||||
config::Cia402ProtocolConfig config_;
|
||||
std::unordered_map<std::uint8_t, NodeState> nodes_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
#endif // CMVR_ES_CIA402_PROTOCOL_H
|
||||
81
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h
vendored
Normal file
81
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h
vendored
Normal file
@ -0,0 +1,81 @@
|
||||
#ifndef CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
|
||||
#define CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "cmvr/msgs/canopen.pb.h"
|
||||
#include "cmvr/msgs/cia402.pb.h"
|
||||
#include "devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
namespace eyou_cia402_pdo_mapping_detail {
|
||||
|
||||
inline constexpr std::uint32_t VENDOR_ID = 0x00001097;
|
||||
inline constexpr std::uint32_t PRODUCT_CODE = 0x00002406;
|
||||
|
||||
inline EthercatPdoEntryConfig entry(const std::uint16_t index,
|
||||
const std::uint8_t subindex,
|
||||
const std::uint8_t bit_len,
|
||||
std::string name)
|
||||
{
|
||||
EthercatPdoEntryConfig cfg;
|
||||
cfg.index = index;
|
||||
cfg.subindex = subindex;
|
||||
cfg.bit_len = bit_len;
|
||||
cfg.name = std::move(name);
|
||||
cfg.padding = index == 0 || bit_len == 0;
|
||||
return cfg;
|
||||
}
|
||||
|
||||
} // namespace eyou_cia402_pdo_mapping_detail
|
||||
|
||||
inline EthercatPdoMapping createEyouCia402PdoMapping()
|
||||
{
|
||||
using namespace eyou_cia402_pdo_mapping_detail;
|
||||
|
||||
EthercatPdoConfig rx_pdo;
|
||||
rx_pdo.index = msgs::CANOPEN_RPDO2_MAP_1601;
|
||||
rx_pdo.sync_manager = 2;
|
||||
rx_pdo.rx = true;
|
||||
rx_pdo.entries = {
|
||||
entry(msgs::CIA402_CONTROL_WORD_6040, 0x00, 16, "Control Word"),
|
||||
entry(msgs::CIA402_TARGET_POSITION_607A, 0x00, 32, "Target Position"),
|
||||
entry(msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, 32, "Target Velocity"),
|
||||
entry(msgs::CIA402_TARGET_TORQUE_6071, 0x00, 16, "Target Torque"),
|
||||
entry(msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, 32, "Profile Acceleration"),
|
||||
entry(msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, 32, "Profile Deceleration"),
|
||||
entry(msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, 32, "Profile Velocity"),
|
||||
entry(msgs::CIA402_TORQUE_SLOPE_6087, 0x00, 32, "Torque Slope"),
|
||||
entry(msgs::CIA402_OPERATION_MODE_6060, 0x00, 8, "Mode Of Operation"),
|
||||
entry(0x0000, 0x00, 8, "Padding"),
|
||||
};
|
||||
|
||||
EthercatPdoConfig tx_pdo;
|
||||
tx_pdo.index = msgs::CANOPEN_TPDO1_MAP_1A00;
|
||||
tx_pdo.sync_manager = 3;
|
||||
tx_pdo.rx = false;
|
||||
tx_pdo.entries = {
|
||||
entry(msgs::CIA402_STATUS_WORD_6041, 0x00, 16, "Status Word"),
|
||||
entry(msgs::CIA402_ACTUAL_POSITION_6064, 0x00, 32, "Actual Position"),
|
||||
entry(msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, 32, "Actual Velocity"),
|
||||
entry(msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, 16, "Actual Torque"),
|
||||
entry(msgs::CIA402_MODE_DISPLAY_6061, 0x00, 8, "Mode Of Operation Display"),
|
||||
entry(msgs::CIA402_ERROR_CODE_603F, 0x00, 16, "Error Code"),
|
||||
entry(0x0000, 0x00, 8, "Padding"),
|
||||
};
|
||||
|
||||
EthercatPdoMapping mapping;
|
||||
mapping.vendor_id = VENDOR_ID;
|
||||
mapping.product_code = PRODUCT_CODE;
|
||||
mapping.name = "EYOU ServoModule ECAT V145 CiA402";
|
||||
mapping.rx_pdos.push_back(std::move(rx_pdo));
|
||||
mapping.tx_pdos.push_back(std::move(tx_pdo));
|
||||
return mapping;
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
#endif // CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
|
||||
43
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h
vendored
Normal file
43
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h
vendored
Normal file
@ -0,0 +1,43 @@
|
||||
#ifndef CMVR_ES_EYOU_MOTOR_H
|
||||
#define CMVR_ES_EYOU_MOTOR_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/config/motor_config/motor_config.pb.h"
|
||||
#include "devices/motor/abstract_motor.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
class EyouMotor final : public AbstractMotor {
|
||||
public:
|
||||
EyouMotor(const config::MotorConfigItem& config,
|
||||
std::shared_ptr<Cia402Protocol> cia402_protocol,
|
||||
std::unique_ptr<EyouMotorAdapter> vendor_adapter);
|
||||
|
||||
std::string typeName() const override { return "EyouMotor"; }
|
||||
bool init() override;
|
||||
void setLimitQ(double ub, double lb) override;
|
||||
void setLimitQd(double qd) override;
|
||||
bool calibrateZeroQ() override;
|
||||
bool brakeRelease() override;
|
||||
|
||||
private:
|
||||
bool hasDependencies_() const;
|
||||
bool hasValidConversion_() const;
|
||||
bool writeVendorPositionLimits_() const;
|
||||
bool writeVendorVelocityLimit_() const;
|
||||
std::int32_t radToCounts_(double angle_rad) const;
|
||||
std::uint32_t radPerSecToCounts_(double velocity_rad_s) const;
|
||||
|
||||
std::shared_ptr<Cia402Protocol> cia402_protocol_;
|
||||
std::unique_ptr<EyouMotorAdapter> vendor_adapter_;
|
||||
double encoder_counts_per_rev_{0.0};
|
||||
double gear_ratio_{0.0};
|
||||
};
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
#endif // CMVR_ES_EYOU_MOTOR_H
|
||||
33
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h
vendored
Normal file
33
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h
vendored
Normal file
@ -0,0 +1,33 @@
|
||||
#ifndef CMVR_ES_EYOU_MOTOR_ADAPTER_H
|
||||
#define CMVR_ES_EYOU_MOTOR_ADAPTER_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
|
||||
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
class EyouMotorAdapter final : public MotorVendorAdapter {
|
||||
public:
|
||||
explicit EyouMotorAdapter(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime);
|
||||
~EyouMotorAdapter() override = default;
|
||||
|
||||
bool initNode(std::uint8_t node_id) override;
|
||||
bool writePositionLimits(std::uint8_t node_id,
|
||||
std::int32_t lower_limit,
|
||||
std::int32_t upper_limit) override;
|
||||
bool writeVelocityLimit(std::uint8_t node_id,
|
||||
std::uint32_t velocity_limit) override;
|
||||
bool calibrateZero(std::uint8_t node_id,
|
||||
std::int32_t& zeroed_position) override;
|
||||
bool brakeRelease(std::uint8_t node_id) override;
|
||||
|
||||
private:
|
||||
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
#endif // CMVR_ES_EYOU_MOTOR_ADAPTER_H
|
||||
20
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h
vendored
Normal file
20
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h
vendored
Normal file
@ -0,0 +1,20 @@
|
||||
#ifndef CMVR_ES_EYOU_OBJECTS_H
|
||||
#define CMVR_ES_EYOU_OBJECTS_H
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace cmvr::device::eyou {
|
||||
|
||||
inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003;
|
||||
|
||||
inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014;
|
||||
|
||||
inline constexpr std::uint16_t EYOU_OVER_SPEED_THRESHOLD_2024 = 0x2024;
|
||||
inline constexpr std::uint16_t EYOU_FIRST_ENCODER_VALUE_202A = 0x202A;
|
||||
inline constexpr std::uint16_t EYOU_SECOND_ENCODER_VALUE_202B = 0x202B;
|
||||
|
||||
inline constexpr std::uint16_t EYOU_STORE_PARAMETERS_1010 = 0x1010;
|
||||
|
||||
} // namespace cmvr::device::eyou
|
||||
|
||||
#endif // CMVR_ES_EYOU_OBJECTS_H
|
||||
25
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h
vendored
Normal file
25
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h
vendored
Normal file
@ -0,0 +1,25 @@
|
||||
#ifndef CMVR_ES_MOTOR_VENDOR_ADAPTER_H
|
||||
#define CMVR_ES_MOTOR_VENDOR_ADAPTER_H
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
class MotorVendorAdapter {
|
||||
public:
|
||||
virtual ~MotorVendorAdapter() = default;
|
||||
|
||||
virtual bool initNode(std::uint8_t node_id) = 0;
|
||||
virtual bool writePositionLimits(std::uint8_t node_id,
|
||||
std::int32_t lower_limit,
|
||||
std::int32_t upper_limit) = 0;
|
||||
virtual bool writeVelocityLimit(std::uint8_t node_id,
|
||||
std::uint32_t velocity_limit) = 0;
|
||||
virtual bool calibrateZero(std::uint8_t node_id,
|
||||
std::int32_t& zeroed_position) = 0;
|
||||
virtual bool brakeRelease(std::uint8_t node_id) = 0;
|
||||
};
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
#endif // CMVR_ES_MOTOR_VENDOR_ADAPTER_H
|
||||
@ -0,0 +1,872 @@
|
||||
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
#include "cmvr/msgs/cia402.pb.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
Cia402Protocol::Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
|
||||
const config::Cia402ProtocolConfig& config)
|
||||
: bus_runtime_(std::move(bus_runtime)),
|
||||
config_(config)
|
||||
{
|
||||
comm_proto = CommProto::ETHERCAT;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::initNode(const std::uint8_t node_id)
|
||||
{
|
||||
if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) {
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] missing EtherCAT motor: "
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!validateNodePdos_(node_id)) {
|
||||
return false;
|
||||
}
|
||||
auto& state = nodeState_(node_id);
|
||||
std::int32_t actual_position = 0;
|
||||
if (readActualPosition_(node_id, actual_position)) {
|
||||
state.target_position = actual_position;
|
||||
}
|
||||
writeNode_(node_id, state);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id,
|
||||
const double target_q,
|
||||
const double max_qd,
|
||||
const double max_qdd)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
if (!hasValidConversion_(node_id, state)) {
|
||||
return false;
|
||||
}
|
||||
state.target_position = radToCounts_(target_q, state);
|
||||
state.profile_velocity = std::abs(radPerSecToCounts_(max_qd, state));
|
||||
if (max_qdd > 0.0) {
|
||||
const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state));
|
||||
state.profile_acceleration = profile_acceleration;
|
||||
state.profile_deceleration = profile_acceleration;
|
||||
}
|
||||
writeProfilePositionTarget_(node_id, state);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id,
|
||||
const double target_qd,
|
||||
const double max_qdd)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
if (!hasValidConversion_(node_id, state)) {
|
||||
return false;
|
||||
}
|
||||
state.target_velocity = radPerSecToCounts_(target_qd, state);
|
||||
if (max_qdd > 0.0) {
|
||||
const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state));
|
||||
state.profile_acceleration = profile_acceleration;
|
||||
state.profile_deceleration = profile_acceleration;
|
||||
}
|
||||
writeNode_(node_id, state);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::commandCyclicPosition(const std::uint8_t node_id,
|
||||
const double target_q,
|
||||
const double target_qd)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
if (!hasValidConversion_(node_id, state)) {
|
||||
return false;
|
||||
}
|
||||
state.target_position = radToCounts_(target_q, state);
|
||||
state.target_velocity = radPerSecToCounts_(target_qd, state);
|
||||
writeNode_(node_id, state);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::commandCyclicVelocity(const std::uint8_t node_id,
|
||||
const double target_qd)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
if (!hasValidConversion_(node_id, state)) {
|
||||
return false;
|
||||
}
|
||||
state.target_velocity = radPerSecToCounts_(target_qd, state);
|
||||
writeNode_(node_id, state);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id,
|
||||
const double target_tau)
|
||||
{
|
||||
(void)target_tau;
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] cyclic torque command is not implemented, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
void Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
if (!prepareSafeTargetsForMode_(node_id, mode, state)) {
|
||||
return;
|
||||
}
|
||||
writeTargetsForMode_(node_id, mode, state);
|
||||
state.mode = mode;
|
||||
writeNode_(node_id, state);
|
||||
}
|
||||
|
||||
msgs::RunMode Cia402Protocol::getMode(const std::uint8_t node_id)
|
||||
{
|
||||
std::int8_t mode_display = 0;
|
||||
if (readModeDisplay_(node_id, mode_display)) {
|
||||
return fromCia402Mode_(mode_display);
|
||||
}
|
||||
return nodeState_(node_id).mode;
|
||||
}
|
||||
|
||||
void Cia402Protocol::setLimitQdd(const std::uint8_t node_id,
|
||||
const double u_qdd,
|
||||
const double l_qdd)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
state.limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd));
|
||||
if (hasValidConversion_(node_id, state)) {
|
||||
const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state));
|
||||
state.profile_acceleration = profile_acceleration;
|
||||
state.profile_deceleration = profile_acceleration;
|
||||
writeAccelerationLimitsToDictionary_(node_id, state);
|
||||
}
|
||||
}
|
||||
|
||||
void Cia402Protocol::setLimitQd(const std::uint8_t node_id, const double qd)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
state.limit_qd = std::abs(qd);
|
||||
if (hasValidConversion_(node_id, state)) {
|
||||
state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state));
|
||||
writeVelocityLimitToDictionary_(node_id, state);
|
||||
}
|
||||
}
|
||||
|
||||
void Cia402Protocol::setLimitQ(const std::uint8_t node_id,
|
||||
const double ub,
|
||||
const double lb)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
state.limit_q_ub = ub;
|
||||
state.limit_q_lb = lb;
|
||||
if (hasValidConversion_(node_id, state)) {
|
||||
writePositionLimitsToDictionary_(node_id, state);
|
||||
}
|
||||
}
|
||||
|
||||
bool Cia402Protocol::calibrateZeroQ(const std::uint8_t node_id)
|
||||
{
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] zero calibration is vendor-specific, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::reachedTargetQ(const std::uint8_t node_id)
|
||||
{
|
||||
std::uint16_t statusword = 0;
|
||||
if (!readStatusword_(node_id, statusword)) {
|
||||
return false;
|
||||
}
|
||||
return targetReached_(cia402::statusword(statusword));
|
||||
}
|
||||
|
||||
void Cia402Protocol::setMotorConversion(
|
||||
const std::uint8_t node_id,
|
||||
const double encoder_counts_per_rev,
|
||||
const double gear_ratio)
|
||||
{
|
||||
auto& state = nodeState_(node_id);
|
||||
state.encoder_counts_per_rev = encoder_counts_per_rev;
|
||||
state.gear_ratio = gear_ratio;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::torqueOn(const std::uint8_t node_id)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto& state = nodeState_(node_id);
|
||||
std::uint16_t statusword = 0;
|
||||
if (readStatusword_(node_id, statusword) && cia402::statusword(statusword).fault != 0) {
|
||||
if (!writeControlword_(node_id, cia402::faultResetControlword())) {
|
||||
return false;
|
||||
}
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(config_.status_poll_period_ms()));
|
||||
}
|
||||
|
||||
if (!prepareSafeTargetsForMode_(node_id, msgs::RUN_MODE_PROFILE_POSITION, state)) {
|
||||
return false;
|
||||
}
|
||||
state.mode = msgs::RUN_MODE_PROFILE_POSITION;
|
||||
state.target_velocity = 0;
|
||||
state.target_torque = 0;
|
||||
writeTargetsForMode_(node_id, state.mode, state);
|
||||
if (!bus_runtime_->writePdo<std::int8_t>(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00,
|
||||
toCia402Mode_(state.mode))) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
|
||||
cia402::DeviceState::ReadyToSwitchOn,
|
||||
"Ready To Switch On")) {
|
||||
return false;
|
||||
}
|
||||
if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(),
|
||||
cia402::DeviceState::SwitchedOn,
|
||||
"Switched On")) {
|
||||
return false;
|
||||
}
|
||||
if (!writeControlwordAndWait_(node_id, cia402::enableOperationControlword(),
|
||||
cia402::DeviceState::OperationEnabled,
|
||||
"Operation Enabled")) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::int32_t actual_position = 0;
|
||||
if (readActualPosition_(node_id, actual_position)) {
|
||||
state.target_position = actual_position;
|
||||
}
|
||||
writeTargetsForMode_(node_id, state.mode, state);
|
||||
|
||||
state.controlword = cia402::profilePositionControlword(true);
|
||||
if (!writeControlword_(node_id, state.controlword)) {
|
||||
return false;
|
||||
}
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(config_.profile_position_trigger_delay_ms()));
|
||||
|
||||
state.controlword = cia402::profilePositionControlword(false);
|
||||
return writeControlword_(node_id, state.controlword);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::torqueOff(const std::uint8_t node_id)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto& state = nodeState_(node_id);
|
||||
std::int32_t actual_position = 0;
|
||||
if (readActualPosition_(node_id, actual_position)) {
|
||||
state.target_position = actual_position;
|
||||
}
|
||||
state.target_velocity = 0;
|
||||
state.target_torque = 0;
|
||||
writeTargetsForMode_(node_id, state.mode, state);
|
||||
waitVelocityNearZero_(node_id, "torqueOff");
|
||||
|
||||
if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(),
|
||||
cia402::DeviceState::SwitchedOn,
|
||||
"Switched On")) {
|
||||
return false;
|
||||
}
|
||||
if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
|
||||
cia402::DeviceState::ReadyToSwitchOn,
|
||||
"Ready To Switch On")) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::brakeRelease(const std::uint8_t node_id)
|
||||
{
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] brake release is vendor-specific, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::quickStop(const std::uint8_t node_id)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto& state = nodeState_(node_id);
|
||||
state.target_velocity = 0;
|
||||
state.target_torque = 0;
|
||||
writeTargetsForMode_(node_id, state.mode, state);
|
||||
|
||||
state.controlword = cia402::quickStopControlword();
|
||||
if (!writeControlword_(node_id, state.controlword)) {
|
||||
return false;
|
||||
}
|
||||
return waitVelocityNearZero_(node_id, "quickStop");
|
||||
}
|
||||
|
||||
double Cia402Protocol::getQ(const std::uint8_t node_id)
|
||||
{
|
||||
std::int32_t actual_position = 0;
|
||||
if (!readActualPosition_(node_id, actual_position)) {
|
||||
return 0.0;
|
||||
}
|
||||
const auto& state = nodeState_(node_id);
|
||||
if (!hasValidConversion_(node_id, state)) {
|
||||
return 0.0;
|
||||
}
|
||||
return countsToRad_(actual_position, state);
|
||||
}
|
||||
|
||||
double Cia402Protocol::getQd(const std::uint8_t node_id)
|
||||
{
|
||||
std::int32_t actual_velocity = 0;
|
||||
if (!readActualVelocity_(node_id, actual_velocity)) {
|
||||
return 0.0;
|
||||
}
|
||||
const auto& state = nodeState_(node_id);
|
||||
if (!hasValidConversion_(node_id, state)) {
|
||||
return 0.0;
|
||||
}
|
||||
return countsToRadPerSec_(actual_velocity, state);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::syncTargetToActualPosition(const std::uint8_t node_id)
|
||||
{
|
||||
std::int32_t actual_position = 0;
|
||||
if (!readActualPosition_(node_id, actual_position)) {
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual position, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
auto& state = nodeState_(node_id);
|
||||
state.target_position = actual_position;
|
||||
state.target_velocity = 0;
|
||||
state.target_torque = 0;
|
||||
writeTargetsForMode_(node_id, state.mode, state);
|
||||
return true;
|
||||
}
|
||||
|
||||
std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode)
|
||||
{
|
||||
switch (mode) {
|
||||
case msgs::RUN_MODE_PROFILE_POSITION:
|
||||
return 1;
|
||||
case msgs::RUN_MODE_PROFILE_VELOCITY:
|
||||
return 3;
|
||||
case msgs::RUN_MODE_HOMING:
|
||||
return 6;
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
|
||||
return 8;
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
|
||||
return 9;
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT:
|
||||
return 10;
|
||||
default:
|
||||
return 8;
|
||||
}
|
||||
}
|
||||
|
||||
msgs::RunMode Cia402Protocol::fromCia402Mode_(const std::int8_t mode)
|
||||
{
|
||||
switch (mode) {
|
||||
case 1:
|
||||
return msgs::RUN_MODE_PROFILE_POSITION;
|
||||
case 3:
|
||||
return msgs::RUN_MODE_PROFILE_VELOCITY;
|
||||
case 6:
|
||||
return msgs::RUN_MODE_HOMING;
|
||||
case 8:
|
||||
return msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
|
||||
case 9:
|
||||
return msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
|
||||
case 10:
|
||||
return msgs::RUN_MODE_CYCLIC_SYNC_CURRENT;
|
||||
default:
|
||||
return msgs::RUN_MODE_UNSPECIFIED;
|
||||
}
|
||||
}
|
||||
|
||||
cia402::Controlword Cia402Protocol::nextControlword_(const cia402::Statusword statusword)
|
||||
{
|
||||
if (statusword.fault != 0) {
|
||||
return cia402::faultResetControlword();
|
||||
}
|
||||
if (cia402::hasState(statusword, cia402::DeviceState::SwitchOnDisabled)) {
|
||||
return cia402::shutdownControlword();
|
||||
}
|
||||
if (cia402::hasState(statusword, cia402::DeviceState::ReadyToSwitchOn)) {
|
||||
return cia402::switchOnControlword();
|
||||
}
|
||||
if (cia402::hasState(statusword, cia402::DeviceState::SwitchedOn)) {
|
||||
return cia402::enableOperationControlword();
|
||||
}
|
||||
if (cia402::hasState(statusword, cia402::DeviceState::OperationEnabled)) {
|
||||
return cia402::enableOperationControlword();
|
||||
}
|
||||
return cia402::shutdownControlword();
|
||||
}
|
||||
|
||||
bool Cia402Protocol::isOperationEnabled_(const cia402::Statusword statusword)
|
||||
{
|
||||
return cia402::isOperationEnabled(statusword);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::targetReached_(const cia402::Statusword statusword)
|
||||
{
|
||||
return cia402::targetReached(statusword);
|
||||
}
|
||||
|
||||
std::int32_t Cia402Protocol::radToCounts_(const double angle_rad,
|
||||
const NodeState& state) const
|
||||
{
|
||||
const double rev = angle_rad / (2.0 * M_PI);
|
||||
return static_cast<std::int32_t>(
|
||||
std::llround(rev * state.gear_ratio * state.encoder_counts_per_rev));
|
||||
}
|
||||
|
||||
double Cia402Protocol::countsToRad_(const std::int32_t counts,
|
||||
const NodeState& state) const
|
||||
{
|
||||
return static_cast<double>(counts) /
|
||||
(state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI;
|
||||
}
|
||||
|
||||
std::int32_t Cia402Protocol::radPerSecToCounts_(
|
||||
const double velocity_rad_s,
|
||||
const NodeState& state) const
|
||||
{
|
||||
const double rev_per_sec = velocity_rad_s / (2.0 * M_PI);
|
||||
return static_cast<std::int32_t>(
|
||||
std::llround(rev_per_sec * state.gear_ratio * state.encoder_counts_per_rev));
|
||||
}
|
||||
|
||||
std::int32_t Cia402Protocol::radPerSec2ToCounts_(
|
||||
const double acceleration_rad_s2,
|
||||
const NodeState& state) const
|
||||
{
|
||||
const double rev_per_sec2 = acceleration_rad_s2 / (2.0 * M_PI);
|
||||
return static_cast<std::int32_t>(
|
||||
std::llround(rev_per_sec2 * state.gear_ratio * state.encoder_counts_per_rev));
|
||||
}
|
||||
|
||||
double Cia402Protocol::countsToRadPerSec_(
|
||||
const std::int32_t velocity_counts_s,
|
||||
const NodeState& state) const
|
||||
{
|
||||
return static_cast<double>(velocity_counts_s) /
|
||||
(state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI;
|
||||
}
|
||||
|
||||
Cia402Protocol::NodeState& Cia402Protocol::nodeState_(const std::uint8_t node_id)
|
||||
{
|
||||
return nodes_[node_id];
|
||||
}
|
||||
|
||||
const Cia402Protocol::NodeState* Cia402Protocol::findNodeState_(
|
||||
const std::uint8_t node_id) const
|
||||
{
|
||||
const auto it = nodes_.find(node_id);
|
||||
if (it == nodes_.end()) {
|
||||
return nullptr;
|
||||
}
|
||||
return &it->second;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::hasValidConversion_(const std::uint8_t node_id,
|
||||
const NodeState& state) const
|
||||
{
|
||||
if (state.encoder_counts_per_rev > 0.0 && state.gear_ratio > 0.0) {
|
||||
return true;
|
||||
}
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] missing conversion config for node "
|
||||
<< static_cast<int>(node_id)
|
||||
<< ": encoder_counts_per_rev=" << state.encoder_counts_per_rev
|
||||
<< ", gear_ratio=" << state.gear_ratio;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::validateNodePdos_(const std::uint8_t node_id) const
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
struct RequiredEntry {
|
||||
std::uint16_t index;
|
||||
std::uint8_t subindex;
|
||||
const char* name;
|
||||
};
|
||||
const RequiredEntry required[] = {
|
||||
{msgs::CIA402_CONTROL_WORD_6040, 0x00, "controlword"},
|
||||
{msgs::CIA402_TARGET_POSITION_607A, 0x00, "target position"},
|
||||
{msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, "target velocity"},
|
||||
{msgs::CIA402_TARGET_TORQUE_6071, 0x00, "target torque"},
|
||||
{msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, "profile acceleration"},
|
||||
{msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, "profile deceleration"},
|
||||
{msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, "profile velocity"},
|
||||
{msgs::CIA402_OPERATION_MODE_6060, 0x00, "operation mode"},
|
||||
{msgs::CIA402_STATUS_WORD_6041, 0x00, "statusword"},
|
||||
{msgs::CIA402_ACTUAL_POSITION_6064, 0x00, "actual position"},
|
||||
{msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, "actual velocity"},
|
||||
{msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, "actual torque"},
|
||||
{msgs::CIA402_MODE_DISPLAY_6061, 0x00, "mode display"},
|
||||
{msgs::CIA402_ERROR_CODE_603F, 0x00, "error code"},
|
||||
};
|
||||
|
||||
for (const auto& entry : required) {
|
||||
if (!bus_runtime_->hasPdoEntry(node_id, entry.index, entry.subindex)) {
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] missing PDO entry for node "
|
||||
<< static_cast<int>(node_id)
|
||||
<< ": " << entry.name
|
||||
<< " 0x" << std::hex << entry.index
|
||||
<< ":" << static_cast<int>(entry.subindex) << std::dec;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::readStatusword_(const std::uint8_t node_id,
|
||||
std::uint16_t& statusword) const
|
||||
{
|
||||
return bus_runtime_ &&
|
||||
bus_runtime_->readPdo<std::uint16_t>(node_id, msgs::CIA402_STATUS_WORD_6041, 0x00,
|
||||
statusword);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::readActualPosition_(const std::uint8_t node_id,
|
||||
std::int32_t& actual_position) const
|
||||
{
|
||||
return bus_runtime_ &&
|
||||
bus_runtime_->readPdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00,
|
||||
actual_position);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::readActualVelocity_(const std::uint8_t node_id,
|
||||
std::int32_t& actual_velocity) const
|
||||
{
|
||||
return bus_runtime_ &&
|
||||
bus_runtime_->readPdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00,
|
||||
actual_velocity);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::readModeDisplay_(const std::uint8_t node_id,
|
||||
std::int8_t& mode_display) const
|
||||
{
|
||||
return bus_runtime_ &&
|
||||
bus_runtime_->readPdo<std::int8_t>(node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00,
|
||||
mode_display);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::writeControlword_(const std::uint8_t node_id,
|
||||
const cia402::Controlword controlword)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
auto& state = nodeState_(node_id);
|
||||
state.controlword = controlword;
|
||||
return bus_runtime_->writePdo<std::uint16_t>(node_id, msgs::CIA402_CONTROL_WORD_6040,
|
||||
0x00, controlword.value);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::writeControlwordAndWait_(
|
||||
const std::uint8_t node_id,
|
||||
const cia402::Controlword controlword,
|
||||
const cia402::DeviceState target_state,
|
||||
const char* state_name)
|
||||
{
|
||||
if (!writeControlword_(node_id, controlword)) {
|
||||
return false;
|
||||
}
|
||||
return waitStatus_(node_id, target_state, state_name);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::waitStatus_(const std::uint8_t node_id,
|
||||
const cia402::DeviceState target_state,
|
||||
const char* state_name) const
|
||||
{
|
||||
const auto deadline = std::chrono::steady_clock::now() +
|
||||
std::chrono::milliseconds(config_.state_transition_timeout_ms());
|
||||
std::uint16_t last_statusword = 0;
|
||||
do {
|
||||
if (readStatusword_(node_id, last_statusword) &&
|
||||
cia402::hasState(cia402::statusword(last_statusword), target_state)) {
|
||||
return true;
|
||||
}
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(config_.status_poll_period_ms()));
|
||||
} while (std::chrono::steady_clock::now() < deadline);
|
||||
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting for "
|
||||
<< state_name << ", node=" << static_cast<int>(node_id)
|
||||
<< ", last_statusword=0x" << std::hex << last_statusword << std::dec;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::waitVelocityNearZero_(const std::uint8_t node_id,
|
||||
const char* action_name) const
|
||||
{
|
||||
const auto* state = findNodeState_(node_id);
|
||||
if (state == nullptr || !hasValidConversion_(node_id, *state)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto tolerance_counts = std::max<std::int32_t>(
|
||||
1, std::abs(radPerSecToCounts_(config_.stopped_velocity_tolerance_rad_s(), *state)));
|
||||
const auto deadline = std::chrono::steady_clock::now() +
|
||||
std::chrono::milliseconds(config_.velocity_stop_timeout_ms());
|
||||
std::int32_t last_velocity = 0;
|
||||
do {
|
||||
if (readActualVelocity_(node_id, last_velocity) &&
|
||||
std::abs(last_velocity) <= tolerance_counts) {
|
||||
return true;
|
||||
}
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(config_.status_poll_period_ms()));
|
||||
} while (std::chrono::steady_clock::now() < deadline);
|
||||
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting velocity near zero "
|
||||
<< "during " << action_name
|
||||
<< ", node=" << static_cast<int>(node_id)
|
||||
<< ", last_velocity=" << last_velocity
|
||||
<< ", tolerance=" << tolerance_counts;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::writePositionLimitsToDictionary_(
|
||||
const std::uint8_t node_id,
|
||||
const NodeState& state) const
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
if (!std::isfinite(state.limit_q_lb) || !std::isfinite(state.limit_q_ub) ||
|
||||
state.limit_q_ub <= state.limit_q_lb) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const auto lower_limit = radToCounts_(state.limit_q_lb, state);
|
||||
const auto upper_limit = radToCounts_(state.limit_q_ub, state);
|
||||
|
||||
const bool ok =
|
||||
bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
|
||||
0x01, lower_limit) &&
|
||||
bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
|
||||
0x02, upper_limit);
|
||||
if (!ok) {
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write software position "
|
||||
<< "limits to dictionary, node=" << static_cast<int>(node_id)
|
||||
<< ", lower=" << lower_limit
|
||||
<< ", upper=" << upper_limit;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::writeVelocityLimitToDictionary_(
|
||||
const std::uint8_t node_id,
|
||||
const NodeState& state) const
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
if (!std::isfinite(state.limit_qd) || state.limit_qd <= 0.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const auto velocity_limit =
|
||||
static_cast<std::uint32_t>(std::abs(radPerSecToCounts_(state.limit_qd, state)));
|
||||
const bool ok =
|
||||
bus_runtime_->writeSdo<std::uint32_t>(node_id, msgs::CIA402_MAX_PROFILE_VELOCITY_607F,
|
||||
0x00, velocity_limit);
|
||||
if (!ok) {
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write velocity limit "
|
||||
<< "to dictionary, node=" << static_cast<int>(node_id)
|
||||
<< ", velocity_limit=" << velocity_limit;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool Cia402Protocol::writeAccelerationLimitsToDictionary_(
|
||||
const std::uint8_t node_id,
|
||||
const NodeState& state) const
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
if (!std::isfinite(state.limit_qdd) || state.limit_qdd <= 0.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const auto acceleration_limit =
|
||||
static_cast<std::uint32_t>(std::abs(radPerSec2ToCounts_(state.limit_qdd, state)));
|
||||
const bool ok =
|
||||
bus_runtime_->writeSdo<std::uint32_t>(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
|
||||
0x00, acceleration_limit) &&
|
||||
bus_runtime_->writeSdo<std::uint32_t>(node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
|
||||
0x00, acceleration_limit);
|
||||
if (!ok) {
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write acceleration limits "
|
||||
<< "to dictionary, node=" << static_cast<int>(node_id)
|
||||
<< ", acceleration_limit=" << acceleration_limit;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id,
|
||||
NodeState& state)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (state.profile_velocity <= 0 && state.limit_qd > 0.0) {
|
||||
state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state));
|
||||
}
|
||||
if (state.profile_acceleration <= 0 && state.limit_qdd > 0.0) {
|
||||
const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state));
|
||||
state.profile_acceleration = profile_acceleration;
|
||||
state.profile_deceleration = profile_acceleration;
|
||||
}
|
||||
|
||||
state.controlword = cia402::enableOperationControlword();
|
||||
writeNode_(node_id, state);
|
||||
|
||||
state.controlword = cia402::profilePositionControlword(true);
|
||||
writeControlword_(node_id, state.controlword);
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(config_.profile_position_trigger_delay_ms()));
|
||||
|
||||
state.controlword = cia402::profilePositionControlword(false);
|
||||
writeControlword_(node_id, state.controlword);
|
||||
}
|
||||
|
||||
bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id,
|
||||
const msgs::RunMode mode,
|
||||
NodeState& state)
|
||||
{
|
||||
state.target_velocity = 0;
|
||||
state.target_torque = 0;
|
||||
|
||||
switch (mode) {
|
||||
case msgs::RUN_MODE_PROFILE_POSITION:
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: {
|
||||
std::int32_t actual_position = 0;
|
||||
if (!readActualPosition_(node_id, actual_position)) {
|
||||
CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual "
|
||||
<< "position before switching mode, node="
|
||||
<< static_cast<int>(node_id)
|
||||
<< ", mode=" << static_cast<int>(mode);
|
||||
return false;
|
||||
}
|
||||
state.target_position = actual_position;
|
||||
break;
|
||||
}
|
||||
|
||||
case msgs::RUN_MODE_PROFILE_VELOCITY:
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT:
|
||||
case msgs::RUN_MODE_HOMING:
|
||||
case msgs::RUN_MODE_UNSPECIFIED:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id,
|
||||
const msgs::RunMode mode,
|
||||
const NodeState& state) const
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (mode) {
|
||||
case msgs::RUN_MODE_PROFILE_POSITION:
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_POSITION_607A,
|
||||
0x00, state.target_position);
|
||||
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, 0x00)) {
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_VELOCITY_6081,
|
||||
0x00, state.profile_velocity);
|
||||
}
|
||||
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) {
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
|
||||
0x00, state.profile_acceleration);
|
||||
}
|
||||
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) {
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
|
||||
0x00, state.profile_deceleration);
|
||||
}
|
||||
break;
|
||||
|
||||
case msgs::RUN_MODE_PROFILE_VELOCITY:
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
|
||||
0x00, state.target_velocity);
|
||||
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) {
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
|
||||
0x00, state.profile_acceleration);
|
||||
}
|
||||
if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) {
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
|
||||
0x00, state.profile_deceleration);
|
||||
}
|
||||
break;
|
||||
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_POSITION_607A,
|
||||
0x00, state.target_position);
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
|
||||
0x00, state.target_velocity);
|
||||
break;
|
||||
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
|
||||
bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
|
||||
0x00, state.target_velocity);
|
||||
break;
|
||||
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT:
|
||||
bus_runtime_->writePdo<std::int16_t>(node_id, msgs::CIA402_TARGET_TORQUE_6071,
|
||||
0x00, state.target_torque);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::uint16_t statusword = 0;
|
||||
if (readStatusword_(node_id, statusword)) {
|
||||
const auto status = cia402::statusword(statusword);
|
||||
state.controlword = nextControlword_(status);
|
||||
std::int32_t actual_position = 0;
|
||||
if (!isOperationEnabled_(status) &&
|
||||
readActualPosition_(node_id, actual_position) &&
|
||||
actual_position != 0) {
|
||||
state.target_position = actual_position;
|
||||
}
|
||||
}
|
||||
|
||||
bus_runtime_->writePdo<std::uint16_t>(node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00,
|
||||
state.controlword.value);
|
||||
bus_runtime_->writePdo<std::int8_t>(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00,
|
||||
toCia402Mode_(state.mode));
|
||||
|
||||
writeTargetsForMode_(node_id, state.mode, state);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
174
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp
vendored
Normal file
174
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp
vendored
Normal file
@ -0,0 +1,174 @@
|
||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <utility>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
EyouMotor::EyouMotor(const config::MotorConfigItem& config,
|
||||
std::shared_ptr<Cia402Protocol> cia402_protocol,
|
||||
std::unique_ptr<EyouMotorAdapter> vendor_adapter)
|
||||
: cia402_protocol_(std::move(cia402_protocol)),
|
||||
vendor_adapter_(std::move(vendor_adapter))
|
||||
{
|
||||
info_.id = config.id();
|
||||
info_.joint_name = config.joint_name();
|
||||
info_.limit_q_lb = config.limit_q_lb();
|
||||
info_.limit_q_ub = config.limit_q_ub();
|
||||
info_.limit_qd = config.limit_qd();
|
||||
info_.limit_qdd = config.limit_qdd();
|
||||
encoder_counts_per_rev_ = config.encoder_counts_per_rev();
|
||||
gear_ratio_ = config.gear_ratio();
|
||||
node_id_ = static_cast<std::uint8_t>(info_.id);
|
||||
id_ = info_.joint_name;
|
||||
protocol_ = cia402_protocol_;
|
||||
}
|
||||
|
||||
bool EyouMotor::init()
|
||||
{
|
||||
std::scoped_lock lock(mtx_);
|
||||
if (!hasDependencies_() || !hasValidConversion_()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!cia402_protocol_->initNode(node_id_)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotor] failed to init CiA402 node: " << info_.joint_name;
|
||||
return false;
|
||||
}
|
||||
if (!vendor_adapter_->initNode(node_id_)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name;
|
||||
return false;
|
||||
}
|
||||
cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_);
|
||||
|
||||
cia402_protocol_->setLimitQd(node_id_, info_.limit_qd);
|
||||
if (!writeVendorVelocityLimit_()) {
|
||||
return false;
|
||||
}
|
||||
if (info_.limit_qdd > 0.0) {
|
||||
cia402_protocol_->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
|
||||
}
|
||||
if (!writeVendorPositionLimits_()) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void EyouMotor::setLimitQ(const double ub, const double lb)
|
||||
{
|
||||
std::scoped_lock lock(mtx_);
|
||||
info_.limit_q_ub = ub;
|
||||
info_.limit_q_lb = lb;
|
||||
if (!hasDependencies_() || !hasValidConversion_()) {
|
||||
return;
|
||||
}
|
||||
writeVendorPositionLimits_();
|
||||
}
|
||||
|
||||
void EyouMotor::setLimitQd(const double qd)
|
||||
{
|
||||
std::scoped_lock lock(mtx_);
|
||||
info_.limit_qd = qd;
|
||||
if (!hasDependencies_() || !hasValidConversion_()) {
|
||||
return;
|
||||
}
|
||||
cia402_protocol_->setLimitQd(node_id_, info_.limit_qd);
|
||||
writeVendorVelocityLimit_();
|
||||
}
|
||||
|
||||
bool EyouMotor::calibrateZeroQ()
|
||||
{
|
||||
std::scoped_lock lock(mtx_);
|
||||
if (!hasDependencies_() || !hasValidConversion_()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::int32_t zeroed_position = 0;
|
||||
if (!vendor_adapter_->calibrateZero(node_id_, zeroed_position)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name;
|
||||
return false;
|
||||
}
|
||||
if (!cia402_protocol_->syncTargetToActualPosition(node_id_)) {
|
||||
return false;
|
||||
}
|
||||
if (!writeVendorPositionLimits_()) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool EyouMotor::brakeRelease()
|
||||
{
|
||||
std::scoped_lock lock(mtx_);
|
||||
if (!hasDependencies_()) {
|
||||
return false;
|
||||
}
|
||||
return vendor_adapter_->brakeRelease(node_id_);
|
||||
}
|
||||
|
||||
bool EyouMotor::hasDependencies_() const
|
||||
{
|
||||
if (!cia402_protocol_ || !vendor_adapter_) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotor] missing protocol or vendor adapter: "
|
||||
<< info_.joint_name;
|
||||
return false;
|
||||
}
|
||||
if (cia402_protocol_->comm_proto != MotorProtocolInterface::CommProto::ETHERCAT) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotor] invalid protocol for motor: " << info_.joint_name;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool EyouMotor::hasValidConversion_() const
|
||||
{
|
||||
if (encoder_counts_per_rev_ > 0.0 && gear_ratio_ > 0.0) {
|
||||
return true;
|
||||
}
|
||||
CMVR_LOG(ERROR) << "[EyouMotor] missing encoder conversion config: "
|
||||
<< info_.joint_name
|
||||
<< ", encoder_counts_per_rev=" << encoder_counts_per_rev_
|
||||
<< ", gear_ratio=" << gear_ratio_;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool EyouMotor::writeVendorPositionLimits_() const
|
||||
{
|
||||
if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) ||
|
||||
info_.limit_q_ub <= info_.limit_q_lb) {
|
||||
return true;
|
||||
}
|
||||
|
||||
cia402_protocol_->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
|
||||
return vendor_adapter_->writePositionLimits(node_id_,
|
||||
radToCounts_(info_.limit_q_lb),
|
||||
radToCounts_(info_.limit_q_ub));
|
||||
}
|
||||
|
||||
bool EyouMotor::writeVendorVelocityLimit_() const
|
||||
{
|
||||
if (!std::isfinite(info_.limit_qd) || info_.limit_qd <= 0.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd));
|
||||
}
|
||||
|
||||
std::int32_t EyouMotor::radToCounts_(const double angle_rad) const
|
||||
{
|
||||
const double rev = angle_rad / (2.0 * M_PI);
|
||||
return static_cast<std::int32_t>(
|
||||
std::llround(rev * gear_ratio_ * encoder_counts_per_rev_));
|
||||
}
|
||||
|
||||
std::uint32_t EyouMotor::radPerSecToCounts_(const double velocity_rad_s) const
|
||||
{
|
||||
const double rev_per_sec = std::abs(velocity_rad_s) / (2.0 * M_PI);
|
||||
return static_cast<std::uint32_t>(
|
||||
std::llround(rev_per_sec * gear_ratio_ * encoder_counts_per_rev_));
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
221
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp
vendored
Normal file
221
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp
vendored
Normal file
@ -0,0 +1,221 @@
|
||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <limits>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "cmvr/msgs/cia402.pb.h"
|
||||
#include "common/base/logging/logger.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
EyouMotorAdapter::EyouMotorAdapter(
|
||||
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime)
|
||||
: bus_runtime_(std::move(bus_runtime))
|
||||
{
|
||||
}
|
||||
|
||||
bool EyouMotorAdapter::initNode(const std::uint8_t node_id)
|
||||
{
|
||||
return bus_runtime_ && bus_runtime_->hasMotor(node_id);
|
||||
}
|
||||
|
||||
bool EyouMotorAdapter::writePositionLimits(const std::uint8_t node_id,
|
||||
const std::int32_t lower_limit,
|
||||
const std::int32_t upper_limit)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto write_limits = [&]() {
|
||||
return bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
|
||||
0x00, 0) &&
|
||||
bus_runtime_->writeSdo<std::int32_t>(
|
||||
node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
|
||||
0x02, upper_limit) &&
|
||||
bus_runtime_->writeSdo<std::int32_t>(
|
||||
node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
|
||||
0x01, lower_limit) &&
|
||||
bus_runtime_->writeSdo<std::uint32_t>(
|
||||
node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
|
||||
0x00, 0x4C494D54);
|
||||
};
|
||||
const auto readback_matches = [&]() {
|
||||
std::int32_t actual_lower = 0;
|
||||
std::int32_t actual_upper = 0;
|
||||
return bus_runtime_->readSdo<std::int32_t>(
|
||||
node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
|
||||
0x01, actual_lower) &&
|
||||
bus_runtime_->readSdo<std::int32_t>(
|
||||
node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
|
||||
0x02, actual_upper) &&
|
||||
actual_lower == lower_limit &&
|
||||
actual_upper == upper_limit;
|
||||
};
|
||||
|
||||
const bool ok = write_limits() && readback_matches();
|
||||
if (!ok) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write software position "
|
||||
<< "limits, node=" << static_cast<int>(node_id)
|
||||
<< ", soft_limit_state=" << 0x4C494D54
|
||||
<< ", lower=" << lower_limit
|
||||
<< ", upper=" << upper_limit;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id,
|
||||
const std::uint32_t velocity_limit)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::uint32_t actual_velocity_limit = 0;
|
||||
const bool ok =
|
||||
bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024,
|
||||
0x00, velocity_limit) &&
|
||||
bus_runtime_->readSdo<std::uint32_t>(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024,
|
||||
0x00, actual_velocity_limit) &&
|
||||
actual_velocity_limit == velocity_limit;
|
||||
if (!ok) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write over speed "
|
||||
<< "threshold, node=" << static_cast<int>(node_id)
|
||||
<< ", expected=" << velocity_limit
|
||||
<< ", actual=" << actual_velocity_limit;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id,
|
||||
std::int32_t& zeroed_position)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
|
||||
0x00, 0)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position "
|
||||
<< "limit before home offset calibration, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
|
||||
0x00, 0)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds{50});
|
||||
|
||||
std::int32_t actual_position = 0;
|
||||
if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
|
||||
0x00, actual_position)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
|
||||
<< "after clearing home offset, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (actual_position == std::numeric_limits<std::int32_t>::min()) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home "
|
||||
<< "offset calibration, node=" << static_cast<int>(node_id)
|
||||
<< ", actual_position=" << actual_position;
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto home_offset = static_cast<std::int32_t>(-actual_position);
|
||||
if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
|
||||
0x00, home_offset)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node="
|
||||
<< static_cast<int>(node_id)
|
||||
<< ", home_offset=" << home_offset;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!bus_runtime_->writeSdo<std::uint32_t>(
|
||||
node_id, eyou::EYOU_STORE_PARAMETERS_1010,
|
||||
0x01,
|
||||
0x65766173)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds{50});
|
||||
|
||||
std::int32_t home_offset_readback = 0;
|
||||
if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
|
||||
0x00, home_offset_readback)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
if (home_offset_readback != home_offset) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node="
|
||||
<< static_cast<int>(node_id)
|
||||
<< ", expected=" << home_offset
|
||||
<< ", actual=" << home_offset_readback;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
|
||||
0x00, zeroed_position)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
|
||||
<< "after writing home offset, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
if (std::abs(static_cast<long long>(zeroed_position)) > 10000) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset did not zero actual position, "
|
||||
<< "node=" << static_cast<int>(node_id)
|
||||
<< ", actual_position_before=" << actual_position
|
||||
<< ", home_offset=" << home_offset
|
||||
<< ", home_offset_readback=" << home_offset_readback
|
||||
<< ", actual_position_after=" << zeroed_position
|
||||
<< ", tolerance_counts=" << 10000;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id)
|
||||
{
|
||||
if (!bus_runtime_) {
|
||||
return false;
|
||||
}
|
||||
if (!bus_runtime_->writeSdo<std::uint8_t>(
|
||||
node_id, eyou::EYOU_BRAKE_CONTROL_2014,
|
||||
0x01,
|
||||
1)) {
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to release brake, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto deadline =
|
||||
std::chrono::steady_clock::now() + std::chrono::milliseconds{1000};
|
||||
do {
|
||||
std::uint8_t brake_state = 0;
|
||||
if (bus_runtime_->readSdo<std::uint8_t>(
|
||||
node_id, eyou::EYOU_BRAKE_CONTROL_2014,
|
||||
0x02, brake_state) &&
|
||||
(brake_state == 1 || brake_state == 2)) {
|
||||
return true;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds{10});
|
||||
} while (std::chrono::steady_clock::now() < deadline);
|
||||
|
||||
CMVR_LOG(ERROR) << "[EyouMotorAdapter] brake release timeout, node="
|
||||
<< static_cast<int>(node_id);
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
@ -0,0 +1,88 @@
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
|
||||
#include "common/config/config_files.h"
|
||||
#include "devices/motor/abstract_motor.h"
|
||||
#include "devices/motor/manager/include/motor_manager.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
namespace {
|
||||
|
||||
constexpr const char* kMotorManagerId = "ethercat_motors";
|
||||
constexpr const char* kMotorConfigFile =
|
||||
"devices/motor/ethercat_motors_two_real_test.pb.txt";
|
||||
|
||||
class DeviceManagerDestroyGuard {
|
||||
public:
|
||||
~DeviceManagerDestroyGuard()
|
||||
{
|
||||
DeviceManager::destroyInstance();
|
||||
}
|
||||
};
|
||||
|
||||
config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig()
|
||||
{
|
||||
config::DeviceManagerConfig config;
|
||||
config.set_name("eyou_motor_device_manager_real_test");
|
||||
config.set_version("test");
|
||||
config.set_init_all_motors_when_no_active_joints(true);
|
||||
|
||||
auto* motor_entry = config.add_devices();
|
||||
motor_entry->set_id(kMotorManagerId);
|
||||
motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
|
||||
motor_entry->set_config_file(kMotorConfigFile);
|
||||
motor_entry->set_enable(true);
|
||||
|
||||
return config;
|
||||
}
|
||||
|
||||
void printMotorState(const int motor_id, const std::shared_ptr<AbstractMotor>& motor)
|
||||
{
|
||||
ASSERT_NE(motor, nullptr);
|
||||
std::cout << "motor_id=" << motor_id
|
||||
<< ", joint_name=" << motor->jointName()
|
||||
<< ", q=" << motor->getQ() << " rad"
|
||||
<< ", qd=" << motor->getQd() << " rad/s"
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
|
||||
{
|
||||
ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
|
||||
DeviceManagerDestroyGuard guard;
|
||||
|
||||
auto& device_manager =
|
||||
DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
|
||||
auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
|
||||
ASSERT_NE(motor_manager, nullptr);
|
||||
|
||||
// for (int motor_id = 1; motor_id <= 4; ++motor_id) {
|
||||
// printMotorState(motor_id, motor_manager->getMotor(motor_id));
|
||||
// }
|
||||
|
||||
auto motor = motor_manager->getMotor(6);
|
||||
motor->calibrateZeroQ();
|
||||
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
|
||||
motor->commandProfileVelocity(-2,5);
|
||||
|
||||
|
||||
for (int i = 1; i <= 50; ++i)
|
||||
{
|
||||
auto q = motor->getQ();
|
||||
auto qd = motor->getQd();
|
||||
std::cout << "q=" << q << ", qd=" << qd << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
motor->quickStop();
|
||||
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
548
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp
vendored
Normal file
548
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp
vendored
Normal file
@ -0,0 +1,548 @@
|
||||
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <sstream>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "cmvr/msgs/cia402.pb.h"
|
||||
#include "devices/motor/abstract_motor.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
|
||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
namespace {
|
||||
|
||||
constexpr int kMotorId = 1;
|
||||
constexpr std::chrono::milliseconds kModeSettleDelay{100};
|
||||
constexpr std::chrono::milliseconds kCommandSamplePeriod{100};
|
||||
constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
|
||||
constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000};
|
||||
constexpr double kDefaultGearRatio = 101.0;
|
||||
constexpr double kEncoderCountsPerMotorRev = 65536.0;
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
|
||||
config::MotorGroupConfig createSingleSlaveGroup()
|
||||
{
|
||||
config::MotorGroupConfig group;
|
||||
group.set_id("eyou_motor_real_test");
|
||||
group.set_bus_type(config::MOTOR_BUS_ETHERCAT);
|
||||
group.set_vendor(config::MOTOR_VENDOR_EYOU);
|
||||
group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402);
|
||||
|
||||
auto* ethercat = group.mutable_ethercat();
|
||||
ethercat->set_master_index(0);
|
||||
ethercat->set_cycle_us(1000);
|
||||
|
||||
auto* cia402 = ethercat->mutable_cia402();
|
||||
cia402->set_profile_position_trigger_delay_ms(2);
|
||||
cia402->set_state_transition_timeout_ms(1200);
|
||||
cia402->set_velocity_stop_timeout_ms(2000);
|
||||
cia402->set_status_poll_period_ms(10);
|
||||
cia402->set_stopped_velocity_tolerance_rad_s(0.001);
|
||||
|
||||
auto* slave = ethercat->add_slaves();
|
||||
slave->set_motor_id(kMotorId);
|
||||
slave->set_alias(0);
|
||||
slave->set_position(0);
|
||||
|
||||
return group;
|
||||
}
|
||||
|
||||
class RuntimeStopGuard {
|
||||
public:
|
||||
explicit RuntimeStopGuard(std::shared_ptr<EthercatMotorBusRuntime> runtime)
|
||||
: runtime_(std::move(runtime))
|
||||
{
|
||||
}
|
||||
|
||||
~RuntimeStopGuard()
|
||||
{
|
||||
if (runtime_) {
|
||||
runtime_->stop();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<EthercatMotorBusRuntime> runtime_;
|
||||
};
|
||||
|
||||
std::shared_ptr<EthercatMotorBusRuntime> startRuntime()
|
||||
{
|
||||
auto runtime = std::make_shared<EthercatMotorBusRuntime>();
|
||||
runtime->setPdoMapping(createEyouCia402PdoMapping());
|
||||
if (!runtime->init(createSingleSlaveGroup())) {
|
||||
return nullptr;
|
||||
}
|
||||
if (!runtime->start()) {
|
||||
runtime->stop();
|
||||
return nullptr;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
return runtime;
|
||||
}
|
||||
|
||||
std::shared_ptr<Cia402Protocol> createProtocol(
|
||||
const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
|
||||
{
|
||||
return std::make_shared<Cia402Protocol>(runtime, runtime->config().cia402());
|
||||
}
|
||||
|
||||
config::MotorConfigItem createMotorConfig()
|
||||
{
|
||||
config::MotorConfigItem config;
|
||||
config.set_id(kMotorId);
|
||||
config.set_joint_name("ethercat_test_joint");
|
||||
config.set_limit_q_lb(-6.14);
|
||||
config.set_limit_q_ub(6.14);
|
||||
config.set_limit_qd(10.0);
|
||||
config.set_limit_qdd(10.0);
|
||||
config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev);
|
||||
config.set_gear_ratio(kDefaultGearRatio);
|
||||
return config;
|
||||
}
|
||||
|
||||
std::unique_ptr<AbstractMotor> createMotor(
|
||||
const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
|
||||
{
|
||||
auto motor = std::make_unique<EyouMotor>(
|
||||
createMotorConfig(),
|
||||
createProtocol(runtime),
|
||||
std::make_unique<EyouMotorAdapter>(runtime));
|
||||
if (!motor->init()) {
|
||||
return nullptr;
|
||||
}
|
||||
return motor;
|
||||
}
|
||||
|
||||
void printMotorState(const char* label, AbstractMotor& motor)
|
||||
{
|
||||
std::cout << label
|
||||
<< ": motor_q=" << motor.getQ() << " rad"
|
||||
<< ", motor_qd=" << motor.getQd() << " rad/s"
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
std::string hex16(const std::uint16_t value)
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << "0x" << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
|
||||
<< value;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
void printRawEthercatFeedback(const char* label,
|
||||
const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
|
||||
{
|
||||
std::uint16_t statusword = 0;
|
||||
std::int8_t mode_display = 0;
|
||||
std::int32_t actual_position = 0;
|
||||
std::int32_t actual_velocity = 0;
|
||||
std::int16_t actual_torque = 0;
|
||||
std::uint16_t error_code = 0;
|
||||
|
||||
runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword);
|
||||
runtime->readPdo<std::int8_t>(kMotorId, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display);
|
||||
runtime->readPdo<std::int32_t>(kMotorId, msgs::CIA402_ACTUAL_POSITION_6064, 0x00,
|
||||
actual_position);
|
||||
runtime->readPdo<std::int32_t>(kMotorId, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00,
|
||||
actual_velocity);
|
||||
runtime->readPdo<std::int16_t>(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00,
|
||||
actual_torque);
|
||||
runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code);
|
||||
|
||||
std::cout << label
|
||||
<< ": statusword=" << hex16(statusword)
|
||||
<< ", mode_display=" << static_cast<int>(mode_display)
|
||||
<< ", actual_position=" << actual_position
|
||||
<< ", actual_velocity=" << actual_velocity
|
||||
<< ", actual_torque=" << actual_torque
|
||||
<< ", error_code=" << hex16(error_code)
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
void sampleMotorState(AbstractMotor& motor,
|
||||
const std::chrono::milliseconds duration)
|
||||
{
|
||||
for (auto elapsed = std::chrono::milliseconds{0};
|
||||
elapsed < duration;
|
||||
elapsed += kCommandSamplePeriod) {
|
||||
std::this_thread::sleep_for(kCommandSamplePeriod);
|
||||
std::cout << "t=" << (elapsed + kCommandSamplePeriod).count() << " ms";
|
||||
printMotorState("", motor);
|
||||
}
|
||||
}
|
||||
|
||||
double nearbySafeTarget(const double current_q, const double delta_rad)
|
||||
{
|
||||
return current_q + (current_q > 0.0 ? -std::abs(delta_rad) : std::abs(delta_rad));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(EyouMotorRealTest, ReadMotorStateOnly)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
printMotorState("motor state", *motor);
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
for (int i = 1; i <= 10; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
printMotorState("motor state", *motor);
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOff());
|
||||
printMotorState("before calibrateZeroQ", *motor);
|
||||
ASSERT_TRUE(motor->calibrateZeroQ());
|
||||
printMotorState("after calibrateZeroQ", *motor);
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, CommandProfilePosition)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
|
||||
std::this_thread::sleep_for(kModeSettleDelay);
|
||||
|
||||
ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0));
|
||||
sampleMotorState(*motor, kFeedbackSampleDuration);
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, CommandProfileVelocity)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
|
||||
std::this_thread::sleep_for(kModeSettleDelay);
|
||||
|
||||
std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl;
|
||||
ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0));
|
||||
sampleMotorState(*motor, kFeedbackSampleDuration);
|
||||
|
||||
std::cout << "motor.commandProfileVelocity(0 rad/s, 1.0 rad/s^2)" << std::endl;
|
||||
ASSERT_TRUE(motor->commandProfileVelocity(0.0, 1.0));
|
||||
sampleMotorState(*motor, std::chrono::milliseconds{1000});
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, CommandCyclicPosition)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||||
std::this_thread::sleep_for(kModeSettleDelay);
|
||||
|
||||
const std::chrono::milliseconds trajectory_duration{15000};
|
||||
const double period_s = 6.0;
|
||||
const double amplitude_rad = 3;
|
||||
const double phase_rad = 0.0;
|
||||
const double center_q = motor->getQ();
|
||||
const double omega = 2.0 * kPi / period_s;
|
||||
|
||||
std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q
|
||||
<< " rad, period=" << period_s
|
||||
<< " s, amplitude=" << amplitude_rad
|
||||
<< " rad, phase=" << phase_rad
|
||||
<< " rad, command_period=" << kCyclicCommandPeriod.count()
|
||||
<< " ms" << std::endl;
|
||||
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
const auto total_ticks = trajectory_duration / kCyclicCommandPeriod;
|
||||
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
|
||||
const auto elapsed = tick * kCyclicCommandPeriod;
|
||||
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
|
||||
const double theta = omega * t_s + phase_rad;
|
||||
const double target_q = center_q + amplitude_rad * std::sin(theta);
|
||||
const double target_qd = amplitude_rad * omega * std::cos(theta);
|
||||
|
||||
ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
|
||||
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
|
||||
std::cout << "t=" << elapsed.count()
|
||||
<< " ms, target_q=" << target_q
|
||||
<< " rad, target_qd=" << target_qd
|
||||
<< " rad/s";
|
||||
printMotorState("", *motor);
|
||||
}
|
||||
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, CommandCyclicVelocity)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
|
||||
std::this_thread::sleep_for(kModeSettleDelay);
|
||||
|
||||
const std::chrono::milliseconds trajectory_duration{15000};
|
||||
const double period_s = 6.0;
|
||||
const double velocity_amplitude_rad_s = 5.0;
|
||||
const double phase_rad = 0.0;
|
||||
const double omega = 2.0 * kPi / period_s;
|
||||
|
||||
std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s
|
||||
<< " s, velocity_amplitude=" << velocity_amplitude_rad_s
|
||||
<< " rad/s, phase=" << phase_rad
|
||||
<< " rad, command_period=" << kCyclicCommandPeriod.count()
|
||||
<< " ms" << std::endl;
|
||||
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
const auto total_ticks = trajectory_duration / kCyclicCommandPeriod;
|
||||
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
|
||||
const auto elapsed = tick * kCyclicCommandPeriod;
|
||||
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
|
||||
const double theta = omega * t_s + phase_rad;
|
||||
const double target_qd = velocity_amplitude_rad_s * std::sin(theta);
|
||||
|
||||
ASSERT_TRUE(motor->commandCyclicVelocity(target_qd));
|
||||
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
|
||||
std::cout << "t=" << elapsed.count()
|
||||
<< " ms, target_qd=" << target_qd
|
||||
<< " rad/s";
|
||||
printMotorState("", *motor);
|
||||
}
|
||||
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
|
||||
}
|
||||
|
||||
std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl;
|
||||
ASSERT_TRUE(motor->commandCyclicVelocity(0.0));
|
||||
sampleMotorState(*motor, std::chrono::milliseconds{500});
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOff());
|
||||
ASSERT_TRUE(motor->calibrateZeroQ());
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
|
||||
std::this_thread::sleep_for(kModeSettleDelay);
|
||||
|
||||
const std::chrono::milliseconds run_duration{2000};
|
||||
const double period_s = 6.0;
|
||||
const double velocity_amplitude_rad_s = 4.5;
|
||||
const double phase_rad = 0.0;
|
||||
const double omega = 2.0 * kPi / period_s;
|
||||
|
||||
std::cout << "motor.commandCyclicVelocity(sin), then quickStop at "
|
||||
<< run_duration.count()
|
||||
<< " ms, period=" << period_s
|
||||
<< " s, velocity_amplitude=" << velocity_amplitude_rad_s
|
||||
<< " rad/s, phase=" << phase_rad
|
||||
<< " rad, command_period=" << kCyclicCommandPeriod.count()
|
||||
<< " ms" << std::endl;
|
||||
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
const auto total_ticks = run_duration / kCyclicCommandPeriod;
|
||||
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
|
||||
const auto elapsed = tick * kCyclicCommandPeriod;
|
||||
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
|
||||
const double theta = omega * t_s + phase_rad;
|
||||
const double target_qd = velocity_amplitude_rad_s * std::sin(theta);
|
||||
|
||||
ASSERT_TRUE(motor->commandCyclicVelocity(target_qd));
|
||||
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
|
||||
std::cout << "t=" << elapsed.count()
|
||||
<< " ms, target_qd=" << target_qd
|
||||
<< " rad/s";
|
||||
printMotorState("", *motor);
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
}
|
||||
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
|
||||
}
|
||||
|
||||
std::cout << "motor.quickStop()" << std::endl;
|
||||
ASSERT_TRUE(motor->quickStop());
|
||||
printMotorState("after quickStop", *motor);
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
sampleMotorState(*motor, std::chrono::milliseconds{1000});
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, QuickStopInProfilePosition)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOff());
|
||||
ASSERT_TRUE(motor->calibrateZeroQ());
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
|
||||
std::this_thread::sleep_for(kModeSettleDelay);
|
||||
|
||||
const std::chrono::milliseconds quick_stop_time{1000};
|
||||
const double start_q = motor->getQ();
|
||||
const double target_q = nearbySafeTarget(start_q, 4.0);
|
||||
const double max_qd = 2.0;
|
||||
const double max_qdd = 10.0;
|
||||
|
||||
std::cout << "motor.commandProfilePosition(" << target_q
|
||||
<< " rad, " << max_qd
|
||||
<< " rad/s, " << max_qdd
|
||||
<< " rad/s^2), then quickStop at "
|
||||
<< quick_stop_time.count() << " ms" << std::endl;
|
||||
ASSERT_TRUE(motor->commandProfilePosition(target_q, max_qd, max_qdd));
|
||||
std::cout << "wait " << quick_stop_time.count()
|
||||
<< " ms before quickStop" << std::endl;
|
||||
sampleMotorState(*motor, quick_stop_time);
|
||||
printRawEthercatFeedback("raw feedback before quickStop", runtime);
|
||||
|
||||
std::cout << "motor.quickStop()" << std::endl;
|
||||
ASSERT_TRUE(motor->quickStop());
|
||||
printMotorState("after quickStop", *motor);
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
sampleMotorState(*motor, std::chrono::milliseconds{1000});
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, QuickStopInProfileVelocity)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOff());
|
||||
ASSERT_TRUE(motor->calibrateZeroQ());
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
|
||||
std::this_thread::sleep_for(kModeSettleDelay);
|
||||
|
||||
const std::chrono::milliseconds quick_stop_time{2000};
|
||||
const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0;
|
||||
const double max_qdd = 10.0;
|
||||
|
||||
std::cout << "motor.commandProfileVelocity(" << target_qd
|
||||
<< " rad/s, " << max_qdd
|
||||
<< " rad/s^2), then quickStop at "
|
||||
<< quick_stop_time.count() << " ms" << std::endl;
|
||||
ASSERT_TRUE(motor->commandProfileVelocity(target_qd, max_qdd));
|
||||
std::cout << "wait " << quick_stop_time.count()
|
||||
<< " ms before quickStop" << std::endl;
|
||||
sampleMotorState(*motor, quick_stop_time);
|
||||
printRawEthercatFeedback("raw feedback before quickStop", runtime);
|
||||
|
||||
std::cout << "motor.quickStop()" << std::endl;
|
||||
ASSERT_TRUE(motor->quickStop());
|
||||
printMotorState("after quickStop", *motor);
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
sampleMotorState(*motor, std::chrono::milliseconds{1000});
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
}
|
||||
|
||||
TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
|
||||
{
|
||||
auto runtime = startRuntime();
|
||||
ASSERT_NE(runtime, nullptr);
|
||||
RuntimeStopGuard runtime_guard(runtime);
|
||||
|
||||
auto motor = createMotor(runtime);
|
||||
ASSERT_NE(motor, nullptr);
|
||||
|
||||
ASSERT_TRUE(motor->torqueOff());
|
||||
ASSERT_TRUE(motor->calibrateZeroQ());
|
||||
ASSERT_TRUE(motor->torqueOn());
|
||||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||||
std::this_thread::sleep_for(kModeSettleDelay);
|
||||
|
||||
const std::chrono::milliseconds run_duration{2000};
|
||||
const double start_q = motor->getQ();
|
||||
const double target_qd = start_q > 0.0 ? -2.0 : 2.0;
|
||||
|
||||
std::cout << "motor.commandCyclicPosition(linear), start_q=" << start_q
|
||||
<< " rad, target_qd=" << target_qd
|
||||
<< " rad/s, then quickStop at "
|
||||
<< run_duration.count()
|
||||
<< " ms, command_period=" << kCyclicCommandPeriod.count()
|
||||
<< " ms" << std::endl;
|
||||
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
const auto total_ticks = run_duration / kCyclicCommandPeriod;
|
||||
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
|
||||
const auto elapsed = tick * kCyclicCommandPeriod;
|
||||
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
|
||||
const double target_q = start_q + target_qd * t_s;
|
||||
|
||||
ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
|
||||
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
|
||||
std::cout << "t=" << elapsed.count()
|
||||
<< " ms, target_q=" << target_q
|
||||
<< " rad, target_qd=" << target_qd
|
||||
<< " rad/s";
|
||||
printMotorState("", *motor);
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
}
|
||||
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
|
||||
}
|
||||
|
||||
std::cout << "motor.quickStop()" << std::endl;
|
||||
ASSERT_TRUE(motor->quickStop());
|
||||
printMotorState("after quickStop", *motor);
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
sampleMotorState(*motor, std::chrono::milliseconds{1000});
|
||||
printRawEthercatFeedback("raw feedback", runtime);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
Loading…
Reference in New Issue
Block a user