update robot controller

config/dev_config.xml

@@ -6,7 +6,6 @@
         <CanManager>
             <CanGroup id="LeftArmCAN" channelId="0" enable="true">
                 <MotorProtocolManager>
-<!--                    这里每个协议节点中配置的参数应该是can通信协议中可修改的默认配置，比如钛虎电机默认的加速度，速度，限位-->
                     <Ti5MotorProtocol id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                     <Ti5MotorProtocol id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                     <Ti5MotorProtocol id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>

include/hardware/can/can_group.h

@@ -2,10 +2,10 @@
 // Created by linbo on 2025/10/22.
 //
 
-#ifndef CMVR_ES_CAN_GROUP_H
+#pragma once
-#define CMVR_ES_CAN_GROUP_H
 
 #include "motor_protocol/motorprotocolmanager.h"
+
 namespace cmvr::hardware
 {
     class CanGroup
@@ -14,15 +14,18 @@ namespace cmvr::hardware
         explicit CanGroup(const XmlNode &cfg);
         ~CanGroup() = default;
 
+        void init();
 
     private:
+        XmlNode cfg_;
         std::string id_;
         unsigned int channelId_;
         bool enable_;
         std::shared_ptr<MotorProtocolManager> motor_protocol_manager;
 
+        std::shared_ptr<device::AbstractCanbus> can_client_{nullptr};
+        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>> can_receiver_{nullptr};
+        std::shared_ptr<device::CanSender<msgs::RobotDetail>> can_sender_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail>> message_manager_{nullptr};
     };
 }
-
-
-#endif //CMVR_ES_CAN_GROUP_H

include/hardware/can/motor_protocol/abstractmotorprotocol.h

@@ -5,11 +5,18 @@
 #ifndef CMVR_ES_ABSTRACTMOTORPROTOCOL_H
 #define CMVR_ES_ABSTRACTMOTORPROTOCOL_H
 #include "rapidxml/xml_parser.h"
-
+#include "cmvr/msgs/canopen.pb.h"
+#include "cmvr/msgs/motor.pb.h"
 namespace cmvr::hardware
 {
     class AbstractMotorProtocol
     {
+
+    public:
+        enum class CommProto : uint8_t {
+            CANOPEN = 1,
+            CUSTOM = 2
+        };
     public:
         explicit AbstractMotorProtocol(const XmlNode &cfg)
         {
@@ -20,9 +27,32 @@ namespace cmvr::hardware
             limitQUb_ = cfg.getAttrDefault("limitQUb_",3.14f);
             limitQd = cfg.getAttrDefault("limitQd",3.0f);
         }
-        ~AbstractMotorProtocol() = default;
+        virtual ~AbstractMotorProtocol() = default;
 
-    private:
+        /**
+         * @brief 用于初始化与通讯协议相关的设置
+         * @param node_id 电机Id
+         * @return
+         */
+        virtual bool initNode(uint8_t node_id) = 0;
+
+        virtual void setQ(uint8_t node_id, double angle_rad) = 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;
+        virtual void setLimitQd(uint8_t node_id,double qd) = 0;
+        virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0;
+        virtual bool calibrateZeroQ(uint8_t node_id) = 0;
+        virtual bool reachedTargetQ(uint8_t node_id) = 0;
+        virtual void setQd(uint8_t node_id, double qd) = 0;
+        virtual void setQdd(uint8_t node_id,double qdd) = 0;
+        virtual void brake(uint8_t node_id) = 0;
+        virtual void torqueOff(uint8_t node_id) = 0;
+
+        virtual double getQ(uint8_t node_id) = 0;
+        virtual double getQd(uint8_t node_id) = 0;
+        CommProto comm_proto{CommProto::CANOPEN};
+    protected:
         XmlNode cfg_;
         std::string id_;
         std::string joint_name_;

include/hardware/can/motor_protocol/motorprotocolmanager.h

@@ -8,18 +8,41 @@
 #include <unordered_map>
 #include <memory>
 #include "abstractmotorprotocol.h"
+#include "devices/abstract_canbus.h"
+#include "canbus/can_comm/can_sender.h"
+#include "canbus/can_comm/can_receiver.h"
+#include "canbus/can_comm/message_manager.h"
+#include "cmvr/msgs/robot_detail.pb.h"
 
 namespace cmvr::hardware
 {
     class MotorProtocolManager
     {
     public:
-        explicit MotorProtocolManager(const XmlNode &cfg);
+        struct MotorInfo
+        {
+            std::string node_id;    //电机ID
+            std::string jointName;  //关节名称
+            double limitQLb;        //逆时针限位
+            double limitQUb;        //顺时针限位
+            double limitQd;         //加速度限制
+        };
+    public:
+        explicit MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
+                                std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
         ~MotorProtocolManager() = default;
 
-        std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &joint_name);
+        //根据协议类型获取协议实例对象
+        std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
     private:
+        std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
+
+        std::shared_ptr<AbstractMotorProtocol> ti5Protocol_{nullptr};
+        //协议类型，协议实例对象
         std::unordered_map<std::string, std::shared_ptr<AbstractMotorProtocol>> motor_protocols_;
+        //协议类型，电机参数
+        std::unordered_map<std::string, std::vector<MotorInfo>> motors_;
     };
 }
 

include/hardware/can/motor_protocol/ti5motorprotocol.h

@@ -2,19 +2,109 @@
 // Created by linbo on 2025/10/22.
 //
 
-#ifndef CMVR_ES_TI5MOTOR_H
+#pragma once
-#define CMVR_ES_TI5MOTOR_H
 
 #include "rapidxml/xml_parser.h"
 #include "abstractmotorprotocol.h"
+#include "cmvr/msgs/motor.pb.h"
+#include "motor/motor_protocol_interface.h"
+#include "devices/abstract_canbus.h"
+#include "canbus/can_comm/can_receiver.h"
+#include "canbus/can_comm/can_sender.h"
+#include "canbus/can_comm/message_manager.h"
+#include "cmvr/msgs/error_code.pb.h"
+#include "cmvr/msgs/robot_detail.pb.h"
+
+#include "canbus/canopen/sdo_request_protocol.h"
+#include "canbus/canopen/sync_protocol.h"
+#include "canbus/canopen/nmt_request_protocol.h"
+#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h"
+#include <cmath>
 namespace cmvr::hardware{
     class Ti5MotorProtocol final : public AbstractMotorProtocol {
     public:
         explicit Ti5MotorProtocol(const XmlNode &cfg);
+        Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
+                                std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
+        ~Ti5MotorProtocol() override;
+
+        bool initNode(uint8_t node_id) override;
+
+        void setMode(uint8_t node_id, msgs::RunMode mode) 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;
+        void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override;
+        bool calibrateZeroQ(uint8_t node_id) override;
+        void brake(uint8_t node_id) override;
+        bool reachedTargetQ(uint8_t node_id) override;
+
+        double getQ(uint8_t node_id) override;
+        double getQd(uint8_t node_id) override;
+
+        void setQd(uint8_t node_id, double qd) override;
+        void setQdd(uint8_t node_id, double qdd) override;
+
+        void torqueOff(uint8_t node_id) override;
+
+        void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10);
+        void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index,
+                      msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10);
+
+        void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
+        void configPdo(uint8_t node_id);
+
+        std::unique_ptr<msgs::RobotDetail> GetRobotDetail() {
+            auto data_ptr = std::make_unique<msgs::RobotDetail>();
+            message_manager_->GetSensorData(data_ptr.get());
+            return data_ptr;
+        }
+
+        msgs::RunMode getMode(uint8_t node_id) override {
+            return cur_mode_[node_id];
+        }
+
 
     private:
+        void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
+        void setPPTargetPosByPdo(uint8_t node_id, int32_t pos);
+        void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos);
+        void configTPDO1(uint8_t node_id);
+        void configTPDO2(uint8_t node_id);
+        // 目标位置 607A + 控制字 6040
+        void configRPDO1(uint8_t node_id, bool start);
+        bool waitUntil(std::function<bool()> condition, int timeout_ms) {
+            auto start = std::chrono::steady_clock::now();
+            while (!condition()) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+                if (std::chrono::steady_clock::now() - start > std::chrono::milliseconds(timeout_ms))
+                    return false;
+            }
+            return true;
+        }
+    private:
+        static constexpr double GearRatio = 101.0; // 电机减速比
+        static constexpr double RADTODEG = 180.0 / M_PI;
+        std::shared_ptr<device::AbstractCanbus> can_client_{nullptr};
 
+        // key node_id
+        std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
+        std::unordered_map<uint8_t,uint32_t> last_Qd_{};
+        std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
+        std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
+
+        // nmt
+        device::NmtRequestProtocol<msgs::RobotDetail> *nmt_command_{nullptr};
+
+        //sync
+        device::SyncProtocol<msgs::RobotDetail> *sync_command_{nullptr};
+
+        // sdo
+        std::map<uint8_t, device::SdoRequestProtocol<msgs::RobotDetail> *> sdo_commands_{};
+
+        // rpdo1
+        std::map<uint8_t, device::motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
     };
 }
-
-#endif //CMVR_ES_TI5MOTOR_H

src/devices/robot/controller/abstractcontroller.h

@@ -2,8 +2,7 @@
 // Created by linbo on 2025/10/23.
 //
 
-#ifndef CMVR_ES_ABSTRACTCONTROLLER_H
+#pragma once
-#define CMVR_ES_ABSTRACTCONTROLLER_H
 
 #include <curl/curl.h>
 
@@ -25,13 +24,20 @@ namespace cmvr::device
 
         [[nodiscard]] ControllerState getState() const {return state_;}
 
+        //此处的Json中应该包含目标电机信息，电机id，canGroupId等其他必要参数，用以确定是调用哪个can实例发送消息
+        // 还要包含操作内容，比如要执行的是直接控制每个电机位置，还是指定末端关节位置
+        /*
+         {“canGroupId”:"",motors:[{"joint_name":"","id":""},{"joint_name":"","id":""}]}
+         **/
         virtual void call(const Json::Value& json) = 0;
 
+        virtual void interrupt() = 0;
+
         virtual void stop() = 0;
 
     protected:
         ControllerState state_;
+        double defaultSpeed_;
+        double defaultAcc_;
     };
 }
-
-#endif //CMVR_ES_ABSTRACTCONTROLLER_H

src/devices/robot/controller/cartesiancontroller.cpp

@@ -16,8 +16,23 @@ void CartesianController::call(const Json::Value& json)
 {
     if (state_ != ControllerState_Idle)
         return;
+
+
+    state_ = ControllerState_Executing;
 }
+
+void CartesianController::interrupt()
+{
+    state_ = ControllerState_Switching;
+
+
+    state_ = ControllerState_Idle;
+}
+
 void CartesianController::stop()
 {
 
+
+    state_ = ControllerState_Idle;
+
 }

src/devices/robot/controller/cartesiancontroller.h

@@ -2,8 +2,7 @@
 // Created by linbo on 2025/10/24.
 //
 
-#ifndef CMVR_ES_CARTESIANCONTROLLER_H
+#pragma once
-#define CMVR_ES_CARTESIANCONTROLLER_H
 #include "abstractcontroller.h"
 
 namespace  cmvr::device
@@ -13,12 +12,9 @@ namespace  cmvr::device
     public:
         explicit CartesianController(const XmlNode& cfg);
         void call(const Json::Value& json) override;
+        void interrupt() override;
         void stop() override;
     private:
 
     };
 }
-
-
-
-#endif //CMVR_ES_CARTESIANCONTROLLER_H

src/devices/robot/controller/controller_manager.cpp

@@ -54,3 +54,56 @@ void ControllerManager::create(const XmlNode& cfg)
     }
 }
 
+void ControllerManager::switchMode(ControlManagerState state)
+{
+    //无论当前切换成哪种模式都应该停止控制器操作？
+    if (activeController_)
+    {
+        activeController_->stop();
+    }
+    //切换当前状态
+    switch (state)
+    {
+    case ControlManagerState_Idle:
+        {
+            break;
+        }
+    case ControlManagerState_Command:
+        {
+            break;
+        }
+    case ControlManagerState_MinorFault:
+        {
+            break;
+        }
+    case ControlManagerState_MajorFault:
+        {
+            break;
+        }
+    case ControlManagerState_Teach:
+        {
+            break;
+        }
+    case ControlManagerState_EStop:
+        {
+            break;
+        }
+    default:
+        break;
+    }
+    state_ = state;
+
+
+}
+
+ComponentGroup& ControllerManager::getComponentGroup(const std::string& id)
+{
+    if (componentGroups_.count(id))
+        return componentGroups_[id];
+    throw std::runtime_error("ComponentGroup not found");
+}
+
+std::shared_ptr<AbstractController> ControllerManager::getActiveController()
+{
+    return activeController_;
+}

src/devices/robot/controller/controller_manager.h

@@ -2,8 +2,7 @@
 // Created by linbo on 2025/10/23.
 //
 
-#ifndef CMVR_ES_CONTROLLER_MANAGER_H
+#pragma once
-#define CMVR_ES_CONTROLLER_MANAGER_H
 
 #include "abstractcontroller.h"
 namespace cmvr::device
@@ -34,20 +33,22 @@ namespace cmvr::device
 
         [[nodiscard]] ControlManagerState getSate() const {return state_;}
 
+        [[nodiscard]] ComponentGroup& getComponentGroup(const std::string& id);
         // 清除错误信息
         void clearError();
 
         // 根据xml内容创建控制器组
         void create(const XmlNode& cfg);
+
+        void switchMode(ControlManagerState state);
+
+        std::shared_ptr<AbstractController> getActiveController();
     private:
 
         ControlManagerState state_;
 
         std::unordered_map<std::string,ComponentGroup> componentGroups_;
+
+        std::shared_ptr<AbstractController> activeController_{nullptr};//当前活动控制器
     };
 }
-
-
-
-
-#endif //CMVR_ES_CONTROLLER_MANAGER_H

src/devices/robot/controller/jointpositioncontroller.cpp

@@ -3,20 +3,40 @@
 //
 
 #include "jointpositioncontroller.h"
+#include "hardware/can/can_manager.h"
+#include "hardware_manager/hardware_manager.h"
 using namespace std;
 using namespace cmvr::device;
+using namespace cmvr::hardware;
+
 JointPositionController::JointPositionController(const XmlNode& cfg):AbstractController(cfg)
 {
 
 }
 
-
 void JointPositionController::call(const Json::Value& json)
 {
     if (state_ != ControllerState_Idle)
         return;
+
+
+    //解析jason，执行算法
+
+
+
+    //调用canmanager执行电机指令
+    HardWareManager::getInstance();
 }
+
+void JointPositionController::interrupt()
+{
+    state_ = ControllerState_Switching;
+
+
+    state_ = ControllerState_Idle;
+}
+
 void JointPositionController::stop()
 {
-
+    state_ = ControllerState_Idle;
 }

src/devices/robot/controller/jointpositioncontroller.h

@@ -2,8 +2,7 @@
 // Created by linbo on 2025/10/24.
 //
 
-#ifndef CMVR_ES_JOINTPOSITIONCONTROLLER_H
+#pragma once
-#define CMVR_ES_JOINTPOSITIONCONTROLLER_H
 
 #include "abstractcontroller.h"
 namespace cmvr::device
@@ -13,12 +12,8 @@ namespace cmvr::device
     public:
         explicit JointPositionController(const XmlNode& cfg);
         void call(const Json::Value& json) override;
+        void interrupt() override;
         void stop() override;
     private:
     };
 }
-
-
-
-
-#endif //CMVR_ES_JOINTPOSITIONCONTROLLER_H

src/hardware/can/can_group.cpp

@@ -7,18 +7,47 @@
 using namespace std;
 using namespace cmvr::hardware;
 
-CanGroup::CanGroup(const XmlNode &cfg)
+CanGroup::CanGroup(const XmlNode &cfg):cfg_(cfg)
 {
     try {
         id_ = cfg.getAttrString("id");
         channelId_ = cfg.getAttrDefault("channelId",0);
         enable_ = cfg.getAttrDefault("enable", false);
-        motor_protocol_manager = std::make_shared<MotorProtocolManager>(cfg);
+
     }
     catch (const exception &e) {
         LOG(FATAL) << "[CanGroup] (CanGroup): CanGroup initialization failed: " << e.what();
         throw std::runtime_error("[CanGroup] (CanGroup): CanGroup create failed" + string(e.what()));
     }
+}
+
+
+void CanGroup::init()
+{
+    can_client_ = std::make_shared<device::SocketCanClientRaw>(cfg_);
+    can_sender_ = std::make_shared<device::CanSender<msgs::RobotDetail> >();
+    can_receiver_ = std::make_shared<device::CanReceiver<msgs::RobotDetail> >();
+    message_manager_ = std::make_shared<device::MessageManager<msgs::RobotDetail> >();
+
+    //初始化protocol协议实例
+    can_client_->init();
+    auto ret = can_sender_->Init(can_client_.get(), false);
+    if (ret != device::ErrorCode::OK)
+        LOG(ERROR) << "Failed to init " << id_ << " CAN sender.";
+    ret = can_receiver_->Init(can_client_.get(), message_manager_.get(), false);
+    if (ret != device::ErrorCode::OK)
+        LOG(ERROR) << "Failed to init " << id_ << " CAN receiver.";
+    can_client_->start();
+    ret = can_sender_->Start();
+    if (ret != device::ErrorCode::OK)
+        LOG(ERROR) << "Failed to start " << id_ << " CAN sender.";
+
+    ret = can_receiver_->Start();
+    if (ret != device::ErrorCode::OK)
+        LOG(ERROR) << "Failed to start " << id_ << " CAN receiver.";
+
+
+    motor_protocol_manager = std::make_shared<MotorProtocolManager>(cfg_,can_sender_,message_manager_);
 
 }
 

src/hardware/can/can_manager.cpp

@@ -18,7 +18,10 @@ CanManager::CanManager(const XmlNode& cfg)
                 string id = node.getAttrString("id");
                 if (canGroups_.count(id))
                     throw runtime_error("[CanManager](CanManager): duplicate CanGroup id:" + id);
-                canGroups_[id] = std::make_shared<CanGroup>(cfg);
+                auto canGroup = std::make_shared<CanGroup>(cfg);
+                canGroup->init();
+                canGroups_[id] = canGroup;
+
             }
         }
     }

src/hardware/can/motor_protocol/motorprotocolmanager.cpp

@@ -9,22 +9,31 @@
 using namespace std;
 using namespace cmvr::hardware;
 
-
+MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
-
+                                std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager)
-MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg)
 {
+    can_sender_ = sender;
+    message_manager_ = manager;
     try {
+        if (cfg.hasChild("Ti5MotorProtocol"))
+        {
+            motor_protocols_["Ti5MotorProtocol"] = std::make_shared<Ti5MotorProtocol>(cfg,can_sender_,message_manager_);
+            std::vector<MotorInfo> motors;
             for (auto &node: cfg.getChildren()){
                 if (node.getNodeName() == "Ti5MotorProtocol")
                 {
-                std::string id = node.getAttrString("id");
+                    MotorInfo motor;
-                std::string joint_name = node.getAttrString("joint_name");
+                    motor.node_id = node.getAttrString("id");
-                if (motor_protocols_.count(joint_name))
+                    motor.jointName = node.getAttrString("joint_name");
-                    throw runtime_error("[MotorProtocolManager] (MotorProtocolManager): duplicate motorProtocol joint name:" + joint_name);
+                    motor.limitQd = node.getAttrDefault("limitQd",3.0f);
+                    motor.limitQLb = node.getAttrDefault("limitQLb",3.14f);
+                    motor.limitQUb = node.getAttrDefault("limitQUb",3.14f);
+                    motors.emplace_back(motor);
+                }
+            }
+            motors_["Ti5MotorProtocol"] = motors;
+        }
 
-                motor_protocols_[joint_name] = std::make_shared<Ti5MotorProtocol>(cfg);
-            }
-        }
     }
     catch (const exception &e) {
         LOG(FATAL) << "[MotorProtocolManager] (MotorProtocolManager): MotorProtocolManager initialization failed: " << e.what();
@@ -33,9 +42,9 @@ MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg)
 }
 
 
-std::shared_ptr<AbstractMotorProtocol> MotorProtocolManager::getMotorProtocol(const std::string &joint_name)
+std::shared_ptr<AbstractMotorProtocol> MotorProtocolManager::getMotorProtocol(const std::string &protocolType)
 {
-    if (motor_protocols_.count(joint_name))
+    if (motor_protocols_.count(protocolType))
-        return motor_protocols_[joint_name];
+        return motor_protocols_[protocolType];
     return nullptr;
 }

src/hardware/can/motor_protocol/ti5motorprotocol.cpp

@@ -3,9 +3,494 @@
 //
 
 #include "hardware/can/motor_protocol/ti5motorprotocol.h"
+#include "canbus/canopen/register.h"
+#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h"
+#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h"
+#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h"
+#include "canbus/canopen/nmt_response_protocol.h"
+using namespace std;
+using namespace cmvr::device;
+using namespace cmvr::msgs;
 using namespace cmvr::hardware;
 
 Ti5MotorProtocol::Ti5MotorProtocol(const XmlNode &cfg):AbstractMotorProtocol(cfg)
 {
 
 }
+
+Ti5MotorProtocol::Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr<CanSender<msgs::RobotDetail> > sender,
+                                                 std::shared_ptr<MessageManager<msgs::RobotDetail> > manager)
+    : AbstractMotorProtocol(cfg),can_sender_(sender), message_manager_(manager){
+
+    comm_proto = CommProto::CANOPEN;
+
+    // 添加 message
+
+    // NMT
+    message_manager_->AddSendProtocolData<NmtRequestProtocol<RobotDetail>, false>();
+    //sync
+    message_manager_->AddSendProtocolData<SyncProtocol<RobotDetail>, false>();
+
+
+    // nmt
+    nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail> *>(
+        message_manager_->GetMutableProtocolDataById(NmtRequestProtocol<RobotDetail>::ID));
+
+
+    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<SyncProtocol<RobotDetail> *>(
+        message_manager_->GetMutableProtocolDataById(SyncProtocol<RobotDetail>::ID));
+    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);
+}
+Ti5MotorProtocol::~Ti5MotorProtocol()
+{
+
+}
+bool Ti5MotorProtocol::initNode(uint8_t node_id) {
+    //nmt
+    message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id);
+
+    //sdo
+    message_manager_->AddSendProtocolData<SdoRequestProtocol<RobotDetail>, false>(node_id);
+    message_manager_->AddRecvProtocolData<motor::Ti5MotorSdoResponse, false>(node_id);
+
+    //TPDO
+    message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO1,false>(node_id);
+    message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO2,false>(node_id);
+
+    //RPDO
+    message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1,false>(node_id);
+
+
+
+
+    sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail> *>(
+        message_manager_->GetMutableProtocolDataById(SdoRequestProtocol<RobotDetail>::ID(node_id)));
+
+    if (sdo_commands_[node_id] == nullptr) {
+        LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
+        return ErrorCode::CANBUS_ERROR;
+    }
+    can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true);
+
+
+    // pdo1
+    rpdo1_commands_[node_id] = dynamic_cast<motor::Ti5MotorRPDO1 *>(
+        message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO1::ID(node_id)));
+
+    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);
+
+    return ErrorCode::OK;
+}
+
+
+void Ti5MotorProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index,
+                                             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 Ti5MotorProtocol::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<int32_t>(cmd));
+            break;
+        case RUN_MODE_PROFILE_POSITION:
+            setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
+            break;
+        default:
+            break;
+    }
+}
+
+
+void Ti5MotorProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
+    controlword_t cw = {};
+    cw.switch_on = 1;
+    cw.enable_voltage = 1;
+    cw.enable_operation = 1;
+    cw.quick_stop = 1;
+    cw.change_set_immediately = 1;
+
+    // 1. 设置目标位置
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos);
+
+
+    // 2. 设置触发位（bit4 = 1）
+    cw.new_set_point = 1;
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+
+
+    // 3. 清除触发位（bit4 = 0），准备下一次触发
+    cw.new_set_point = 0;
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+}
+
+void Ti5MotorProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
+    // 触发目标位置运动
+    controlword_t cw;
+    cw.value = 0x0F;
+    cw.new_set_point = 1;
+    cw.change_set_immediately = 1;
+
+    rpdo1_commands_[node_id]->SetTargetPos(pos);
+    rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
+    can_sender_->Update(rpdo1_commands_[node_id]->ID());
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+    cw.new_set_point = 0;
+    rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
+    can_sender_->Update(rpdo1_commands_[node_id]->ID());
+}
+
+void Ti5MotorProtocol::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 Ti5MotorProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
+
+    cur_mode_[node_id] = mode;
+    // 1 : 先设置模式
+    auto data = static_cast<uint32_t>(mode);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data);
+
+
+    // 2 : 状态机步进 —— Shutdown（0x06）
+    controlword_t cw = {};
+    cw.quick_stop = 1;
+    cw.enable_voltage = 1;
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
+
+
+    // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
+    cw.switch_on = 1;
+    cw.enable_operation = 1;
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
+
+
+    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);
+
+
+            // 5 : 触发位置运动（new_set_point 翻转）
+            cw.new_set_point = 1;
+            cw.change_set_immediately = 1;
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+
+            // 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标）
+            cw.new_set_point = 0;
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            break;
+        }
+
+        case RUN_MODE_CYCLIC_SYNC_POSITION: {
+            // 设置目标位置为当前位置
+            auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
+            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
+
+            //3 : 使能   15
+            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_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;
+        }
+        default:
+
+            break;
+    }
+}
+
+void Ti5MotorProtocol::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 Ti5MotorProtocol::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 Ti5MotorProtocol::configTPDO1(uint8_t node_id) {
+    //TDPO1 配置   状态字 和 控制字
+    // 1: 失能 pdo
+    uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0);
+
+    // 2: 配置为异步
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+
+    // 3：配置约束时间 unit:0.1ms
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10);
+
+    // 4 : 配置周期发送时间  unit : ms   0 为 数据改变时发送
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0);
+
+    // 5 ：映射控制字
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1,
+                   CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
+
+    //6 : 映射状态字
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2,
+                   STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
+
+    //7 : 映射模式
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3,
+                   MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
+
+    //8  映射错误码
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4,
+                   ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
+
+    //9  写入该PDO映射对象总个数
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4);
+
+    //10 使能
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31));
+}
+
+
+void Ti5MotorProtocol::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));
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0);
+
+    // 2: 配置为异步
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+
+    // 3：配置约束时间 unit:0.1ms
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100);
+
+    // 4 : 配置周期发送时间  unit : ms
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0);
+
+    // 5 ：映射当前位置
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1,
+                   ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
+
+    //6 : 映射当前速度
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2,
+                   ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32);
+
+    //9  写入该PDO映射对象总个数
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2);
+
+    //10 使能
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
+}
+
+void Ti5MotorProtocol::configRPDO1(uint8_t node_id, bool start) {
+    // 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));
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0);
+
+    // 2: 配置为
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+
+    // // 3：配置约束时间 unit:0.1ms
+    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10);
+    //
+    // // 4 : 配置周期发送时间  unit : ms   0 为 数据改变时发送
+    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0);
+
+    // 5 ：映射位置
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1,
+                   TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
+
+    //6 : 映射控制字
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
+                   CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
+
+
+    if (start) {
+        //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 Ti5MotorProtocol::configPdo(uint8_t node_id) {
+    configTPDO1(node_id);
+    configTPDO2(node_id);
+    configRPDO1(node_id, true);
+
+}
+
+void Ti5MotorProtocol::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 Ti5MotorProtocol::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);
+}
+
+void Ti5MotorProtocol::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;
+
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
+}
+
+
+bool Ti5MotorProtocol::calibrateZeroQ(uint8_t node_id) {
+
+    // 0: 设置控制字为 0x06，确保停机状态
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06,1000);
+
+    // 1: 清除偏置值 0x2008 ← 0
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
+
+    // 2: 等待确认清除成功
+    seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
+    if (!waitUntil([&]() {
+        return GetRobotDetail()->motors().at(node_id).position_offset() == 0;
+    }, 1000)) {
+        LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed";
+        return false;
+    }
+
+    // 3: 读取当前位置 0x6064
+    seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
+    auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
+
+    // 4: 将当前位置写入偏置寄存器
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
+
+    // 5: 保存参数到永久区（0x2000 ← 1）
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1,100);
+
+
+    // 6: 确认写入成功
+    seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
+    if (!waitUntil([&]() {
+        return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos;
+    }, 500)) {
+        return false;
+        LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed";
+    }
+
+    return true;
+}
+
+void Ti5MotorProtocol::brake(uint8_t node_id) {
+
+    // // 开机未使能电机时调用
+    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    // 6 抱闸   0 ： 立即停机 自由
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02,100);
+
+    // 必须要发送 0xf 才能按照6085中设定的减速度减速
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+}
+
+bool Ti5MotorProtocol::reachedTargetQ(uint8_t node_id) {
+    statusword_t st{};
+    st.value = GetRobotDetail()->motors().at(node_id).status_word();
+    return st.target_reached == 1 ;
+}
+
+void Ti5MotorProtocol::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: {
+            auto it = last_Qd_.find(node_id);
+            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: {
+                // 在速度模式下，直接设置目标速度
+                seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
+                break;
+        }
+        default:
+            break;
+    }
+}
+
+void Ti5MotorProtocol::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) {
+        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 Ti5MotorProtocol::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);
+    // 必须要发送 0xf 才能按照6085中设定的减速度减速
+    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+
+    // 停机之后，要重新使能？
+    // cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED;
+}
+
+
+double Ti5MotorProtocol::getQ(uint8_t node_id) {
+    auto data_ptr = std::make_unique<msgs::RobotDetail>();
+    message_manager_->GetSensorData(data_ptr.get());
+    auto cnt =  data_ptr->motors().at(node_id).position();
+    return  (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG);
+}
+
+double Ti5MotorProtocol::getQd(uint8_t node_id) {
+    auto data_ptr = std::make_unique<msgs::RobotDetail>();
+    message_manager_->GetSensorData(data_ptr.get());
+    auto cnt =  data_ptr->motors().at(node_id).speed();
+    return  (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
+}
+
+
+

src/main.cpp

@@ -8,6 +8,7 @@
 #include <iostream>
 #include <glog/logging.h>
 #include "device_manager/device_manager.h"
+#include "hardware_manager/hardware_factory.h"
 #include "monitor/monitor_manager.h"
 #include "service/grpc_service/grpc_camera_service.h"
 #include "service/grpc_service/grpc_system_service.h"
@@ -22,16 +23,21 @@
 #include "service/http_service/httpclient.h"
 #include "json/json.h"
 
+#include "hardware_manager/hardware_manager.h"
+
 void runServer(const XmlNode &cfg){
     using namespace cmvr::device;
     using namespace cmvr::service;
     using namespace cmvr::monitor;
-
+    using namespace cmvr::hardware;
     if (!cfg.hasChild("DeviceManager")){
         LOG(ERROR) << "Device Manager node not found";
         return;
     }
     try {
+        auto hardware_cfg = cfg.getChild("HardwareManager");
+        HardWareManager::getInstance(hardware_cfg);
+
         auto dmgr_cfg = cfg.getChild("DeviceManager");
         DeviceManager::getInstance(dmgr_cfg);