update robot controller
This commit is contained in:
parent
505b373368
commit
82ab7cd887
@ -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"/>
|
||||
|
||||
@ -2,10 +2,10 @@
|
||||
// Created by linbo on 2025/10/22.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_CAN_GROUP_H
|
||||
#define CMVR_ES_CAN_GROUP_H
|
||||
#pragma once
|
||||
|
||||
#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
|
||||
}
|
||||
@ -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_;
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -2,19 +2,109 @@
|
||||
// Created by linbo on 2025/10/22.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_TI5MOTOR_H
|
||||
#define CMVR_ES_TI5MOTOR_H
|
||||
#pragma once
|
||||
|
||||
#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
|
||||
}
|
||||
@ -2,8 +2,7 @@
|
||||
// Created by linbo on 2025/10/23.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_ABSTRACTCONTROLLER_H
|
||||
#define CMVR_ES_ABSTRACTCONTROLLER_H
|
||||
#pragma once
|
||||
|
||||
#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
|
||||
@ -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;
|
||||
|
||||
}
|
||||
@ -2,8 +2,7 @@
|
||||
// Created by linbo on 2025/10/24.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_CARTESIANCONTROLLER_H
|
||||
#define CMVR_ES_CARTESIANCONTROLLER_H
|
||||
#pragma once
|
||||
#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
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
@ -2,8 +2,7 @@
|
||||
// Created by linbo on 2025/10/23.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_CONTROLLER_MANAGER_H
|
||||
#define CMVR_ES_CONTROLLER_MANAGER_H
|
||||
#pragma once
|
||||
|
||||
#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
|
||||
@ -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;
|
||||
}
|
||||
@ -2,8 +2,7 @@
|
||||
// Created by linbo on 2025/10/24.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_JOINTPOSITIONCONTROLLER_H
|
||||
#define CMVR_ES_JOINTPOSITIONCONTROLLER_H
|
||||
#pragma once
|
||||
|
||||
#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
|
||||
@ -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_);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -9,22 +9,31 @@
|
||||
using namespace std;
|
||||
using namespace cmvr::hardware;
|
||||
|
||||
|
||||
|
||||
MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg)
|
||||
MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
|
||||
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager)
|
||||
{
|
||||
can_sender_ = sender;
|
||||
message_manager_ = manager;
|
||||
try {
|
||||
for (auto &node: cfg.getChildren()){
|
||||
if (node.getNodeName() == "Ti5MotorProtocol")
|
||||
{
|
||||
std::string id = node.getAttrString("id");
|
||||
std::string joint_name = node.getAttrString("joint_name");
|
||||
if (motor_protocols_.count(joint_name))
|
||||
throw runtime_error("[MotorProtocolManager] (MotorProtocolManager): duplicate motorProtocol joint name:" + joint_name);
|
||||
|
||||
motor_protocols_[joint_name] = std::make_shared<Ti5MotorProtocol>(cfg);
|
||||
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")
|
||||
{
|
||||
MotorInfo motor;
|
||||
motor.node_id = node.getAttrString("id");
|
||||
motor.jointName = node.getAttrString("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;
|
||||
}
|
||||
|
||||
}
|
||||
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))
|
||||
return motor_protocols_[joint_name];
|
||||
if (motor_protocols_.count(protocolType))
|
||||
return motor_protocols_[protocolType];
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user