From 2065108bc2fc70d6ad33d04ca13d65aeb5bc361b Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Thu, 6 Nov 2025 17:27:37 +0800 Subject: [PATCH] update can_group --- include/hardware/can/can_group.h | 2 ++ .../can/motor_protocol/motorprotocolmanager.h | 3 +++ .../controller/jointpositioncontroller.cpp | 12 +++++------- .../robot/humanoid_robot/humanoid_robot.cpp | 17 +++++++++++++++-- src/hardware/can/can_group.cpp | 4 ++++ .../motor_protocol/motorprotocolmanager.cpp | 18 ++++++++++++++++++ 6 files changed, 47 insertions(+), 9 deletions(-) diff --git a/include/hardware/can/can_group.h b/include/hardware/can/can_group.h index 97e6e856..2161a674 100644 --- a/include/hardware/can/can_group.h +++ b/include/hardware/can/can_group.h @@ -18,6 +18,8 @@ namespace cmvr::hardware std::shared_ptr getMotorProtocol(const std::string &protocolType); uint8_t getNodeId(const std::string& joint_name); + + const MotorProtocolManager::MotorInfo& getMotorInfo(const std::string& joint_name); private: XmlNode cfg_; std::string id_; diff --git a/include/hardware/can/motor_protocol/motorprotocolmanager.h b/include/hardware/can/motor_protocol/motorprotocolmanager.h index 2e79f64a..0e0bb7c4 100644 --- a/include/hardware/can/motor_protocol/motorprotocolmanager.h +++ b/include/hardware/can/motor_protocol/motorprotocolmanager.h @@ -26,6 +26,7 @@ namespace cmvr::hardware double limitQLb; //逆时针限位 double limitQUb; //顺时针限位 double limitQd; //加速度限制 + std::shared_ptr motorProtocol; }; public: explicit MotorProtocolManager(const XmlNode &cfg,std::shared_ptr> sender, @@ -35,6 +36,8 @@ namespace cmvr::hardware uint8_t getNodeId(const std::string& joint_name); //根据协议类型获取协议实例对象 std::shared_ptr getMotorProtocol(const std::string &protocolType); + + const MotorInfo& getMotorInfo(const std::string& joint_name); private: std::shared_ptr > can_sender_{nullptr}; std::shared_ptr > message_manager_{nullptr}; diff --git a/src/devices/robot/controller/jointpositioncontroller.cpp b/src/devices/robot/controller/jointpositioncontroller.cpp index 531427a7..444a236d 100644 --- a/src/devices/robot/controller/jointpositioncontroller.cpp +++ b/src/devices/robot/controller/jointpositioncontroller.cpp @@ -24,7 +24,6 @@ void JointPositionController::call(const Json::Value& json) /* "params":{ “canGroupId”:"leftArm", - "protocolType":"Ti5MotorProtocol", "motors":[{"joint_name":""},{"joint_name":""}] }, "operate":{} @@ -34,11 +33,9 @@ void JointPositionController::call(const Json::Value& json) std::string protocolType; if (json.isMember("params")) { - Json::Value params = json["params"]; + const Json::Value& params = json["params"]; if (params.isMember("canGroupId")) canGroupId = params["canGroupId"].asString(); - if (params.isMember("protocolType")) - protocolType = params["protocolType"].asString(); if (params.isMember("motors")) { for (int i = 0; i < params["motors"].size(); i++) @@ -52,10 +49,11 @@ void JointPositionController::call(const Json::Value& json) { } + //最后计算好了通过下面的接口调用电机通信 + std::string joint_name =""; + auto motorInfo = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorInfo(joint_name); + motorInfo.motorProtocol->setQ(motorInfo.node_id,0.1); - //调用canmanager执行电机指令 - auto protocol = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorProtocol(protocolType); - //这里通过joint_name获取node_id? } void JointPositionController::interrupt() diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 8fa69f44..d5c6384e 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -320,14 +320,27 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double } else if (state == ControlManagerState_Command) { - //当前有命令正在执行，判断优先级？ //先获取当前正在执行的控制器 - auto controller = controller_manager_->getActiveController(); + auto activeController = controller_manager_->getActiveController(); + if (activeController) + { + activeController->stop(); + } + auto group = controller_manager_->getComponentGroup("leftArm"); + if (!group.controllers_.count("JointPositionCtrl")) + throw runtime_error("Controller not found"); + auto controller = group.controllers_["JointPositionCtrl"]; + + Json::Value callJson; Json::Value paramsJson; Json::Value operateJson; //拆分cmd，构建json内容 + paramsJson["canGroupId"] = group.CanGroupIDs[0]; + callJson["params"] = paramsJson; + callJson["operate"] = operateJson; + controller->call(callJson); } for (const auto &j: cmd) { diff --git a/src/hardware/can/can_group.cpp b/src/hardware/can/can_group.cpp index 64e3de73..854336da 100644 --- a/src/hardware/can/can_group.cpp +++ b/src/hardware/can/can_group.cpp @@ -61,3 +61,7 @@ uint8_t CanGroup::getNodeId(const std::string& joint_name) return motor_protocol_manager->getNodeId(joint_name); } +const MotorProtocolManager::MotorInfo& CanGroup::getMotorInfo(const std::string& joint_name) +{ + return motor_protocol_manager->getMotorInfo(joint_name); +} diff --git a/src/hardware/can/motor_protocol/motorprotocolmanager.cpp b/src/hardware/can/motor_protocol/motorprotocolmanager.cpp index e4f2cfd9..db005d90 100644 --- a/src/hardware/can/motor_protocol/motorprotocolmanager.cpp +++ b/src/hardware/can/motor_protocol/motorprotocolmanager.cpp @@ -32,6 +32,7 @@ MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg,std::shared_ptr