From 61699a09df7e34f8430ea59e920f296072016d05 Mon Sep 17 00:00:00 2001 From: lgv Date: Tue, 3 Mar 2026 16:50:25 +0800 Subject: [PATCH] feat: add rpdo2 for CSS --- cmvr-es/common/config/cabin_robot.xml | 12 +- .../socket/socket_can_client_raw.cc | 2 +- .../devices/canbus/can_comm/message_manager.h | 4 +- cmvr-es/devices/motor/abstract_motor.h | 8 + cmvr-es/devices/motor/motor_manager_test.cpp | 31 ++- .../devices/motor/motor_protocol_interface.h | 1 + .../devices/motor/ti5_motor/CMakeLists.txt | 1 + .../canopen/protocol/ti5_motor_rpdo2.cpp | 18 ++ .../canopen/protocol/ti5_motor_rpdo2.h | 53 ++++ .../canopen/ti5_motor_canopen_protocol.cpp | 236 +++++++++--------- .../canopen/ti5_motor_canopen_protocol.h | 7 +- 11 files changed, 240 insertions(+), 133 deletions(-) create mode 100644 cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp create mode 100644 cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml index fa833a13..826862ee 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -50,12 +50,12 @@ - - - - - - + + + + + + diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc index 5438804b..aef0af2c 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc @@ -21,7 +21,7 @@ namespace cmvr { SocketCanClientRaw::SocketCanClientRaw(const XmlNode &cfg) : AbstractCanbus(cfg) { - auto channel_id = cfg.getAttrDefault("channelId", 0); + auto channel_id = cfg.getAttrDefault("channelId", 1); port_ = static_cast(channel_id); interface_ = CANCardParameter::NATIVE; diff --git a/cmvr-es/devices/canbus/can_comm/message_manager.h b/cmvr-es/devices/canbus/can_comm/message_manager.h index f77b656d..409d9983 100644 --- a/cmvr-es/devices/canbus/can_comm/message_manager.h +++ b/cmvr-es/devices/canbus/can_comm/message_manager.h @@ -215,8 +215,8 @@ namespace cmvr { *MessageManager::GetMutableProtocolDataById( const uint32_t message_id) { if (protocol_data_map_.find(message_id) == protocol_data_map_.end()) { - LOG(WARNING) << "Unable to get protocol data because of invalid message_id:" - << Byte::byte_to_hex(message_id); + // LOG(WARNING) << "Unable to get protocol data because of invalid message_id:" + // << Byte::byte_to_hex(message_id); return nullptr; } return protocol_data_map_[message_id]; diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 3f356cc8..710f88d2 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -119,6 +119,14 @@ namespace cmvr::device{ protocol_->setTarget(node_id_,q, qd); } + virtual void setTarget(double qd) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + throw std::runtime_error("Protocol not set for motor"); + } + protocol_->setTarget(node_id_, qd); + } + virtual bool calibrateZeroQ() { std::scoped_lock lock(mtx_); if (!protocol_) { diff --git a/cmvr-es/devices/motor/motor_manager_test.cpp b/cmvr-es/devices/motor/motor_manager_test.cpp index 5bfcb556..28018830 100644 --- a/cmvr-es/devices/motor/motor_manager_test.cpp +++ b/cmvr-es/devices/motor/motor_manager_test.cpp @@ -18,7 +18,7 @@ using namespace cmvr::msgs; TEST(MotorMangerTest,MyTest) { - uint8_t id = 17; + uint8_t id = 22; XmlNode cfg; // 1 === 初始化公共组件 === @@ -65,9 +65,25 @@ TEST(MotorMangerTest,MyTest) { } // 6 === 控制电机 === - auto motor_3 = manager->getMotor("L_SHOULDER_R"); + auto motor_3 = manager->getMotor(id); motor_3->init(); - motor_3->calibrateZeroQ(); + + // motor_3->calibrateZeroQ(); + + + + + motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY); + motor_3->setTarget(-0.5); + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION); + motor_3->setTarget(-3.14,1.6); + + // std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + // + // motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION); + // motor_3->setTarget(0,1.6); + // motor_3->calibrateZeroQ(); // motor_3->setMode(RUN_MODE_PROFILE_POSITION); // motor_3->setLimitQ(30.14,-40.14); // motor_3->setQ(-30); @@ -82,9 +98,14 @@ TEST(MotorMangerTest,MyTest) { // manager->setMode(id,RUN_MODE_CYCLIC_SYNC_POSITION); // manager->setQ(id,3.14); - while (true) { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + auto q = motor_3->getQ(); + auto qd = motor_3->getQd(); + std::cout << q << ", " << qd << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + // motor_3->getQ(q); + } LOG(INFO) << "Testing MotorManger"; diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 0ada73af..634a715c 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -28,6 +28,7 @@ namespace cmvr { virtual void setQ(uint8_t node_id, double angle_rad) = 0; virtual void setTarget(uint8_t node_id, double angle_rad,double vel) = 0; + virtual void setTarget(uint8_t node_id,double vel) = 0; virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0; virtual msgs::RunMode getMode(uint8_t node_id) = 0; virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0; diff --git a/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt b/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt index 12121294..508b88b8 100644 --- a/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/ti5_motor/CMakeLists.txt @@ -3,6 +3,7 @@ add_library(ti5motor SHARED ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_tpdo1.cpp ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_tpdo2.cpp ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_rpdo1.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_rpdo2.cpp ${CMAKE_CURRENT_SOURCE_DIR}/canopen/ti5_motor_canopen_protocol.cpp ${CMAKE_CURRENT_SOURCE_DIR}/ti5_motor.cpp diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp new file mode 100644 index 00000000..6c6b5ff0 --- /dev/null +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp @@ -0,0 +1,18 @@ +// +// Created by lgv on 2026/3/3. +// + +#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h" +#include "glog/logging.h" + +using namespace cmvr::msgs; +using namespace cmvr::device::motor; + + +void Ti5MotorRPDO2::UpdateData(uint8_t *data) { + std::lock_guard lock(mutex_); + data[0] = target_vel_ & 0xFF; + data[1] = target_vel_ >> 8 & 0xFF; + data[2] = target_vel_ >> 16 & 0xFF; + data[3] = target_vel_ >> 24 & 0xFF; +} diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h new file mode 100644 index 00000000..5f22dcab --- /dev/null +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h @@ -0,0 +1,53 @@ +// +// Created by lgv on 2026/3/3. +// + +#pragma once +#include "canbus/can_comm/protocol_data.h" +#include "cmvr/msgs/robot_detail.pb.h" +#include + +namespace cmvr { + namespace device { + namespace motor { + + // 周期速度模式，使用 + class Ti5MotorRPDO2 : public device::ProtocolData { + public: + static constexpr uint32_t BASE_ID = msgs::RPDO2_BASE_ID_300; + + static uint32_t ID(uint8_t node_id) { + return BASE_ID + node_id; + } + uint32_t ID() const{ + return BASE_ID + node_id_; + } + + explicit Ti5MotorRPDO2(uint8_t node_id) : node_id_(node_id) {} + + void UpdateData(uint8_t *data) override; + + int32_t GetLength() const override { + return 0x04; + } + + uint32_t GetPeriod() const override { + return 1000 * 1; // 5 ms + } + + + + void SetTargetVel(int32_t velocity) { + std::lock_guard lock(mutex_); + target_vel_ = velocity; + } + + + private: + mutable std::mutex mutex_; + uint8_t node_id_{0}; + int32_t target_vel_{0}; + }; + } + } +} \ No newline at end of file diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp index e83bbaa8..099ff914 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp @@ -11,10 +11,9 @@ using namespace cmvr::device; using namespace cmvr::msgs; -Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr> sender, - std::shared_ptr> manager) - : can_sender_(sender), message_manager_(manager) -{ +Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr > sender, + std::shared_ptr > manager) + : can_sender_(sender), message_manager_(manager) { comm_proto = CommProto::CANOPEN; // 添加 message @@ -26,28 +25,25 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr*>( + nmt_command_ = dynamic_cast *>( message_manager_->GetMutableProtocolDataById(NmtRequestProtocol::ID)); - if (nmt_command_ == nullptr) - { + if (nmt_command_ == nullptr) { LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; } can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true); // sync - sync_command_ = dynamic_cast*>( + sync_command_ = dynamic_cast *>( message_manager_->GetMutableProtocolDataById(SyncProtocol::ID)); - if (sync_command_ == nullptr) - { + if (sync_command_ == nullptr) { LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; } can_sender_->AddMessage(sync_command_->ID, sync_command_, false); } -bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) -{ +bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { //nmt message_manager_->AddRecvProtocolData, false>(node_id); @@ -61,13 +57,13 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) //RPDO message_manager_->AddSendProtocolData(node_id); + message_manager_->AddSendProtocolData(node_id); - sdo_commands_[node_id] = dynamic_cast*>( + sdo_commands_[node_id] = dynamic_cast *>( message_manager_->GetMutableProtocolDataById(SdoRequestProtocol::ID(node_id))); - if (sdo_commands_[node_id] == nullptr) - { + if (sdo_commands_[node_id] == nullptr) { LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } @@ -75,46 +71,51 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) // pdo1 - rpdo1_commands_[node_id] = dynamic_cast( + rpdo1_commands_[node_id] = dynamic_cast( message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO1::ID(node_id))); - if (rpdo1_commands_[node_id] == nullptr) - { + if (rpdo1_commands_[node_id] == nullptr) { LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; return ErrorCode::CANBUS_ERROR; } can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true); + // rpdo2 + rpdo2_commands_[node_id] = dynamic_cast( + message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO2::ID(node_id))); + + if (rpdo2_commands_[node_id] == nullptr) { + LOG(ERROR) << "Ti5 Motor RPDO2 Protocol does not exist in the MessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + can_sender_->AddMessage(rpdo2_commands_[node_id]->ID(), rpdo2_commands_[node_id], true); + return ErrorCode::OK; } void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index, - uint32_t data, uint32_t delay_ms) -{ + uint32_t data, uint32_t delay_ms) { sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data); can_sender_->Update(sdo_commands_[node_id]->ID()); std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); } -void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) -{ +void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) { auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; - switch (cur_mode_[node_id]) - { - case RUN_MODE_CYCLIC_SYNC_POSITION: - setCSPTargetPosByPdo(node_id, static_cast(cmd)); - break; - case RUN_MODE_PROFILE_POSITION: - setPPTargetPosByPdo(node_id, static_cast(cmd)); - // setPPTargetPosBySdo(node_id,static_cast(cmd)); - break; + switch (cur_mode_[node_id]) { + case RUN_MODE_CYCLIC_SYNC_POSITION: + setCSPTargetPosByPdo(node_id, static_cast(cmd)); + break; + case RUN_MODE_PROFILE_POSITION: + // setPPTargetPosByPdo(node_id, static_cast(cmd)); + setPPTargetPosBySdo(node_id, static_cast(cmd)); + break; } } -void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) -{ +void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) { auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; rpdo1_commands_[node_id]->SetTargetPos(pos_cmd); @@ -123,8 +124,14 @@ void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, doubl } -void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) -{ +void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double vel) { + auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; + rpdo2_commands_[node_id]->SetTargetVel(int16_t(speed)); + can_sender_->Update(rpdo2_commands_[node_id]->ID()); +} + + +void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) { controlword_t cw = {}; cw.switch_on = 1; cw.enable_voltage = 1; @@ -146,8 +153,7 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); } -void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) -{ +void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) { // 触发目标位置运动 controlword_t cw; cw.value = 0x0F; @@ -165,16 +171,14 @@ void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) can_sender_->Update(rpdo1_commands_[node_id]->ID()); } -void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) -{ +void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) { rpdo1_commands_[node_id]->SetTargetPos(pos); rpdo1_commands_[node_id]->SetCtrlWord(0x0F); can_sender_->Update(rpdo1_commands_[node_id]->ID()); } -void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) -{ +void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { cur_mode_[node_id] = mode; // 1 : 先设置模式 auto data = static_cast(mode); @@ -186,6 +190,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) cw.quick_stop = 1; cw.enable_voltage = 1; seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + // configRPDO1(node_id, false); + // configRPDO2(node_id, false); // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F） @@ -194,10 +200,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); - switch (mode) - { - case RUN_MODE_PROFILE_POSITION: - { + switch (mode) { + case RUN_MODE_PROFILE_POSITION: { // 4 : 设置目标位置（为当前位置） auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); @@ -214,8 +218,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) break; } - case RUN_MODE_CYCLIC_SYNC_POSITION: - { + case RUN_MODE_CYCLIC_SYNC_POSITION: { + // configRPDO1(node_id, true); // 设置目标位置为当前位置 auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); @@ -227,45 +231,41 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) break; } - case RUN_MODE_PROFILE_VELOCITY: - { + case RUN_MODE_PROFILE_VELOCITY: { cw.enable_operation = 1; cw.switch_on = 1; seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } - case RUN_MODE_CYCLIC_SYNC_VELOCITY: - { + case RUN_MODE_CYCLIC_SYNC_VELOCITY: { + // configRPDO2(node_id, true); cw.enable_operation = 1; cw.switch_on = 1; seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } - default: - // TODO: Handle unspecified or unknown mode - break; + default: + // TODO: Handle unspecified or unknown mode + break; } } -void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) -{ +void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) { nmt_command_->RequestService(node_id, command); can_sender_->Update(nmt_command_->ID); std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); } -void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) -{ +void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) { seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); } -void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) -{ +void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) { //TDPO1 配置 状态字 和 控制字 // 1: 失能 pdo uint32_t cob_id = TPDO1_BASE_ID_180 + node_id; @@ -305,8 +305,7 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) } -void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) -{ +void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) { // 1: 失能 pdo uint32_t cob_id = TPDO2_BASE_ID_280 + node_id; seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31)); @@ -336,8 +335,7 @@ void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31)); } -void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) -{ +void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool enable) { // 1: 失能 pdo uint32_t cob_id = RPDO1_BASE_ID_200 + node_id; seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31)); @@ -357,45 +355,66 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); //6 : 映射控制字 + + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2, + PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32); + + + //7 写入该PDO映射对象总个数 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2); + + //8 使能 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31)); +} + + +void Ti5MotorCanopenProtocol::configRPDO2(uint8_t node_id, bool enable) { + // 1: 失能 pdo + uint32_t cob_id = RPDO2_BASE_ID_300 + node_id; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 0); + + if (!enable) return; + + // 2: 配置为 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + + + // 5 ：映射位置 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_MAP_1601, SUB_INDEX_1, TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32); - if (start) - { - //7 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2); + //7 写入该PDO映射对象总个数 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 1); - //8 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31)); - } + //8 使能 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31)); } -void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) -{ +void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) { configTPDO1(node_id); configTPDO2(node_id); configRPDO1(node_id, true); + configRPDO2(node_id, true); } -void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) -{ +void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) { auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0; auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0; seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel)); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel)); } -void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd) -{ +void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd) { auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; // seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed); } -void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) -{ +void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) { ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0; lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0; @@ -404,8 +423,7 @@ void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) } -bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) -{ +bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { // 0: 设置控制字为 0x06，确保停机状态 seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000); @@ -414,11 +432,9 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) // 2: 等待确认清除成功 seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0); - if (!waitUntil([&]() - { + if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == 0; - }, 1000)) - { + }, 1000)) { LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed"; return false; } @@ -436,11 +452,9 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) // 6: 确认写入成功 seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); - if (!waitUntil([&]() - { + if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos; - }, 500)) - { + }, 500)) { return false; LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed"; } @@ -448,8 +462,7 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) return true; } -void Ti5MotorCanopenProtocol::brake(uint8_t node_id) -{ +void Ti5MotorCanopenProtocol::brake(uint8_t node_id) { // // 开机未使能电机时调用 // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); // 6 抱闸 0 ： 立即停机 自由 @@ -461,56 +474,47 @@ void Ti5MotorCanopenProtocol::brake(uint8_t node_id) seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); } -bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) -{ +bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { statusword_t st{}; st.value = GetRobotDetail()->motors().at(node_id).status_word(); return st.target_reached == 1; } -void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) -{ +void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) { auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; - switch (cur_mode_[node_id]) - { - case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: - case msgs::RUN_MODE_PROFILE_POSITION: - { + switch (cur_mode_[node_id]) { + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: + case msgs::RUN_MODE_PROFILE_POSITION: { auto it = last_Qd_.find(node_id); - if (it == last_Qd_.end() || it->second != speed) - { + if (it == last_Qd_.end() || it->second != speed) { seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)), 0); last_Qd_[node_id] = speed; } break; } - case msgs::RUN_MODE_PROFILE_VELOCITY: - case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: - { + case msgs::RUN_MODE_PROFILE_VELOCITY: + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: { // 在速度模式下，直接设置目标速度 seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0); break; } - default: - break; + default: + break; } } -void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd) -{ +void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd) { uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0); auto it = last_Qdd_.find(node_id); - if (it == last_Qdd_.end() || it->second != accel) - { + if (it == last_Qdd_.end() || it->second != accel) { seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel); last_Qdd_[node_id] = accel; } } -void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) -{ +void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) { // 0 ： 立即停机 自由 seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0); seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20); @@ -522,16 +526,14 @@ void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) } -double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) -{ +double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) { auto data_ptr = std::make_unique(); message_manager_->GetSensorData(data_ptr.get()); auto cnt = data_ptr->motors().at(node_id).position(); return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG); } -double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) -{ +double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) { auto data_ptr = std::make_unique(); message_manager_->GetSensorData(data_ptr.get()); auto cnt = data_ptr->motors().at(node_id).speed(); diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h index 8a0a7e80..e0b3ac88 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h @@ -16,6 +16,7 @@ #include "canbus/canopen/sync_protocol.h" #include "canbus/canopen/nmt_request_protocol.h" #include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h" +#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h" #include namespace cmvr { @@ -30,7 +31,7 @@ namespace cmvr { void setMode(uint8_t node_id, msgs::RunMode mode); void setTarget(uint8_t node_id, double angle_rad, double vel) override; - + void setTarget(uint8_t node_id, double vel) override; void setQ(uint8_t node_id, double angle_rad) override; void setLimitQ(uint8_t node_id, double ub, double lb) override; void setLimitQd(uint8_t node_id, double qd) override; @@ -91,6 +92,7 @@ namespace cmvr { // rpdo1 std::map rpdo1_commands_{}; + std::map rpdo2_commands_{}; void setPPTargetPosBySdo(uint8_t node_id, int32_t pos); @@ -102,7 +104,8 @@ namespace cmvr { void configTPDO1(uint8_t node_id); void configTPDO2(uint8_t node_id); // 目标位置 607A + 控制字 6040 - void configRPDO1(uint8_t node_id, bool start); + void configRPDO1(uint8_t node_id, bool enable); + void configRPDO2(uint8_t node_id, bool enable); bool waitUntil(std::function condition, int timeout_ms) {