SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add rpdo2 for CSS

Browse Source
This commit is contained in:
lgv 2026-03-03 16:50:25 +08:00
parent e06f8f9f1a
commit 61699a09df
11 changed files with 240 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cmvr-es/common/config/cabin_robot.xml
View File

@ -50,12 +50,12 @@
<Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</LeftArmCan> </LeftArmCan>
<RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP"> <RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/> <!-- <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>-->
<Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/> <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan> </RightArmCan>
<HeadCan id = " " devId = " " channelId ="2" enable="false"> <HeadCan id = " " devId = " " channelId ="2" enable="false">

2
cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc
View File

@ -21,7 +21,7 @@ namespace cmvr {
SocketCanClientRaw::SocketCanClientRaw(const XmlNode &cfg) : AbstractCanbus(cfg) { SocketCanClientRaw::SocketCanClientRaw(const XmlNode &cfg) : AbstractCanbus(cfg) {
auto channel_id = cfg.getAttrDefault("channelId", 0); auto channel_id = cfg.getAttrDefault("channelId", 1);
port_ = static_cast<CANCardParameter::CANChannelId>(channel_id); port_ = static_cast<CANCardParameter::CANChannelId>(channel_id);
interface_ = CANCardParameter::NATIVE; interface_ = CANCardParameter::NATIVE;

4
cmvr-es/devices/canbus/can_comm/message_manager.h
View File

@ -215,8 +215,8 @@ namespace cmvr {
*MessageManager<SensorType>::GetMutableProtocolDataById( *MessageManager<SensorType>::GetMutableProtocolDataById(
const uint32_t message_id) { const uint32_t message_id) {
if (protocol_data_map_.find(message_id) == protocol_data_map_.end()) { if (protocol_data_map_.find(message_id) == protocol_data_map_.end()) {
LOG(WARNING) << "Unable to get protocol data because of invalid message_id:" // LOG(WARNING) << "Unable to get protocol data because of invalid message_id:"
<< Byte::byte_to_hex(message_id); // << Byte::byte_to_hex(message_id);
return nullptr; return nullptr;
} }
return protocol_data_map_[message_id]; return protocol_data_map_[message_id];

8
cmvr-es/devices/motor/abstract_motor.h
View File

@ -119,6 +119,14 @@ namespace cmvr::device{
protocol_->setTarget(node_id_,q, qd); 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() { virtual bool calibrateZeroQ() {
std::scoped_lock lock(mtx_); std::scoped_lock lock(mtx_);
if (!protocol_) { if (!protocol_) {

31
cmvr-es/devices/motor/motor_manager_test.cpp
View File

@ -18,7 +18,7 @@ using namespace cmvr::msgs;
TEST(MotorMangerTest,MyTest) { TEST(MotorMangerTest,MyTest) {
uint8_t id = 17; uint8_t id = 22;
XmlNode cfg; XmlNode cfg;
// 1 === 初始化公共组件 === // 1 === 初始化公共组件 ===
@ -65,9 +65,25 @@ TEST(MotorMangerTest,MyTest) {
} }
// 6 === 控制电机 === // 6 === 控制电机 ===
auto motor_3 = manager->getMotor("L_SHOULDER_R"); auto motor_3 = manager->getMotor(id);
motor_3->init(); 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->setMode(RUN_MODE_PROFILE_POSITION);
// motor_3->setLimitQ(30.14,-40.14); // motor_3->setLimitQ(30.14,-40.14);
// motor_3->setQ(-30); // motor_3->setQ(-30);
@ -82,9 +98,14 @@ TEST(MotorMangerTest,MyTest) {
// manager->setMode(id,RUN_MODE_CYCLIC_SYNC_POSITION); // manager->setMode(id,RUN_MODE_CYCLIC_SYNC_POSITION);
// manager->setQ(id,3.14); // manager->setQ(id,3.14);
while (true) { 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"; LOG(INFO) << "Testing MotorManger";

1
cmvr-es/devices/motor/motor_protocol_interface.h
View File

@ -28,6 +28,7 @@ namespace cmvr {
virtual void setQ(uint8_t node_id, double angle_rad) = 0; 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 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 void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
virtual msgs::RunMode getMode(uint8_t node_id) = 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 setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;

1
cmvr-es/devices/motor/ti5_motor/CMakeLists.txt
View File

@ -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_tpdo1.cpp
${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_tpdo2.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_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}/canopen/ti5_motor_canopen_protocol.cpp
${CMAKE_CURRENT_SOURCE_DIR}/ti5_motor.cpp ${CMAKE_CURRENT_SOURCE_DIR}/ti5_motor.cpp

18
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp Normal file
View File

@ -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<std::mutex> 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;
}

53
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h Normal file
View File

@ -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 <mutex>
namespace cmvr {
namespace device {
namespace motor {
// 周期速度模式，使用
class Ti5MotorRPDO2 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
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<std::mutex> lock(mutex_);
target_vel_ = velocity;
}
private:
mutable std::mutex mutex_;
uint8_t node_id_{0};
int32_t target_vel_{0};
};
}
}
}

192
cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp
View File

@ -13,8 +13,7 @@ using namespace cmvr::msgs;
Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs::RobotDetail> > sender, Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs::RobotDetail> > sender,
std::shared_ptr<MessageManager<msgs::RobotDetail> > manager) std::shared_ptr<MessageManager<msgs::RobotDetail> > manager)
: can_sender_(sender), message_manager_(manager) : can_sender_(sender), message_manager_(manager) {
{
comm_proto = CommProto::CANOPEN; comm_proto = CommProto::CANOPEN;
// 添加 message // 添加 message
@ -30,8 +29,7 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs:
message_manager_->GetMutableProtocolDataById(NmtRequestProtocol<RobotDetail>::ID)); message_manager_->GetMutableProtocolDataById(NmtRequestProtocol<RobotDetail>::ID));
if (nmt_command_ == nullptr) if (nmt_command_ == nullptr) {
{
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
} }
can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true); can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true);
@ -39,15 +37,13 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs:
// sync // sync
sync_command_ = dynamic_cast<SyncProtocol<RobotDetail> *>( sync_command_ = dynamic_cast<SyncProtocol<RobotDetail> *>(
message_manager_->GetMutableProtocolDataById(SyncProtocol<RobotDetail>::ID)); message_manager_->GetMutableProtocolDataById(SyncProtocol<RobotDetail>::ID));
if (sync_command_ == nullptr) if (sync_command_ == nullptr) {
{
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
} }
can_sender_->AddMessage(sync_command_->ID, sync_command_, false); can_sender_->AddMessage(sync_command_->ID, sync_command_, false);
} }
bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
{
//nmt //nmt
message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id); message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id);
@ -61,13 +57,13 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id)
//RPDO //RPDO
message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1, false>(node_id); message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1, false>(node_id);
message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO2, false>(node_id);
sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail> *>( sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail> *>(
message_manager_->GetMutableProtocolDataById(SdoRequestProtocol<RobotDetail>::ID(node_id))); message_manager_->GetMutableProtocolDataById(SdoRequestProtocol<RobotDetail>::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!"; LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR; return ErrorCode::CANBUS_ERROR;
} }
@ -78,43 +74,48 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id)
rpdo1_commands_[node_id] = dynamic_cast<motor::Ti5MotorRPDO1 *>( rpdo1_commands_[node_id] = dynamic_cast<motor::Ti5MotorRPDO1 *>(
message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO1::ID(node_id))); 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!"; LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR; return ErrorCode::CANBUS_ERROR;
} }
can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true); can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true);
// rpdo2
rpdo2_commands_[node_id] = dynamic_cast<motor::Ti5MotorRPDO2 *>(
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; return ErrorCode::OK;
} }
void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index, 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); sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data);
can_sender_->Update(sdo_commands_[node_id]->ID()); can_sender_->Update(sdo_commands_[node_id]->ID());
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); 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; auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
switch (cur_mode_[node_id]) switch (cur_mode_[node_id]) {
{
case RUN_MODE_CYCLIC_SYNC_POSITION: case RUN_MODE_CYCLIC_SYNC_POSITION:
setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd)); setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
break; break;
case RUN_MODE_PROFILE_POSITION: case RUN_MODE_PROFILE_POSITION:
setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd)); // setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
// setPPTargetPosBySdo(node_id,static_cast<int32_t>(cmd)); setPPTargetPosBySdo(node_id, static_cast<int32_t>(cmd));
break; 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 pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
rpdo1_commands_[node_id]->SetTargetPos(pos_cmd); 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 = {}; controlword_t cw = {};
cw.switch_on = 1; cw.switch_on = 1;
cw.enable_voltage = 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); 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; controlword_t cw;
cw.value = 0x0F; 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()); 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]->SetTargetPos(pos);
rpdo1_commands_[node_id]->SetCtrlWord(0x0F); rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
can_sender_->Update(rpdo1_commands_[node_id]->ID()); 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; cur_mode_[node_id] = mode;
// 1 : 先设置模式 // 1 : 先设置模式
auto data = static_cast<uint32_t>(mode); auto data = static_cast<uint32_t>(mode);
@ -186,6 +190,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
cw.quick_stop = 1; cw.quick_stop = 1;
cw.enable_voltage = 1; cw.enable_voltage = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); 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） // 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); seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
switch (mode) switch (mode) {
{ case RUN_MODE_PROFILE_POSITION: {
case RUN_MODE_PROFILE_POSITION:
{
// 4 : 设置目标位置（为当前位置） // 4 : 设置目标位置（为当前位置）
auto cur_pos = GetRobotDetail()->motors().at(node_id).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); 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; 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(); auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
@ -227,16 +231,15 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
break; break;
} }
case RUN_MODE_PROFILE_VELOCITY: case RUN_MODE_PROFILE_VELOCITY: {
{
cw.enable_operation = 1; cw.enable_operation = 1;
cw.switch_on = 1; cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break; break;
} }
case RUN_MODE_CYCLIC_SYNC_VELOCITY: case RUN_MODE_CYCLIC_SYNC_VELOCITY: {
{ // configRPDO2(node_id, true);
cw.enable_operation = 1; cw.enable_operation = 1;
cw.switch_on = 1; cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
@ -248,24 +251,21 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
} }
} }
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); nmt_command_->RequestService(node_id, command);
can_sender_->Update(nmt_command_->ID); can_sender_->Update(nmt_command_->ID);
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); 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_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_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); 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 配置 状态字 和 控制字 //TDPO1 配置 状态字 和 控制字
// 1: 失能 pdo // 1: 失能 pdo
uint32_t cob_id = TPDO1_BASE_ID_180 + node_id; 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 // 1: 失能 pdo
uint32_t cob_id = TPDO2_BASE_ID_280 + node_id; 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_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)); 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 // 1: 失能 pdo
uint32_t cob_id = RPDO1_BASE_ID_200 + node_id; 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_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); TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
//6 : 映射控制字 //6 : 映射控制字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2, seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32); PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32);
if (start)
{
//7 写入该PDO映射对象总个数 //7 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2); seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2);
//8 使能 //8 使能
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31)); 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);
//7 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 1);
//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); configTPDO1(node_id);
configTPDO2(node_id); configTPDO2(node_id);
configRPDO1(node_id, true); 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 accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
auto decel = ((l_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_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel)); 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; 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, MAX_SPEED_607F, SUB_INDEX_0, speed);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, 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; ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0;
lb = (lb * 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，确保停机状态 // 0: 设置控制字为 0x06，确保停机状态
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000); 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: 等待确认清除成功 // 2: 等待确认清除成功
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0); 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; return GetRobotDetail()->motors().at(node_id).position_offset() == 0;
}, 1000)) }, 1000)) {
{
LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed"; LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed";
return false; return false;
} }
@ -436,11 +452,9 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id)
// 6: 确认写入成功 // 6: 确认写入成功
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); 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; return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos;
}, 500)) }, 500)) {
{
return false; return false;
LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed"; LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed";
} }
@ -448,8 +462,7 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id)
return true; 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); // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
// 6 抱闸 0 ： 立即停机 自由 // 6 抱闸 0 ： 立即停机 自由
@ -461,24 +474,19 @@ void Ti5MotorCanopenProtocol::brake(uint8_t node_id)
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); 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{}; statusword_t st{};
st.value = GetRobotDetail()->motors().at(node_id).status_word(); st.value = GetRobotDetail()->motors().at(node_id).status_word();
return st.target_reached == 1; 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; auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
switch (cur_mode_[node_id]) switch (cur_mode_[node_id]) {
{
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
case msgs::RUN_MODE_PROFILE_POSITION: case msgs::RUN_MODE_PROFILE_POSITION: {
{
auto it = last_Qd_.find(node_id); 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)), seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),
0); 0);
last_Qd_[node_id] = speed; last_Qd_[node_id] = speed;
@ -486,8 +494,7 @@ void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd)
break; break;
} }
case msgs::RUN_MODE_PROFILE_VELOCITY: case msgs::RUN_MODE_PROFILE_VELOCITY:
case msgs::RUN_MODE_CYCLIC_SYNC_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); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
break; break;
@ -497,20 +504,17 @@ void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd)
} }
} }
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); uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0);
auto it = last_Qdd_.find(node_id); 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_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel);
last_Qdd_[node_id] = accel; last_Qdd_[node_id] = accel;
} }
} }
void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
{
// 0 ： 立即停机 自由 // 0 ： 立即停机 自由
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 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); 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<msgs::RobotDetail>(); auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get()); message_manager_->GetSensorData(data_ptr.get());
auto cnt = data_ptr->motors().at(node_id).position(); auto cnt = data_ptr->motors().at(node_id).position();
return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG); 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<msgs::RobotDetail>(); auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get()); message_manager_->GetSensorData(data_ptr.get());
auto cnt = data_ptr->motors().at(node_id).speed(); auto cnt = data_ptr->motors().at(node_id).speed();

7
cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h
View File

@ -16,6 +16,7 @@
#include "canbus/canopen/sync_protocol.h" #include "canbus/canopen/sync_protocol.h"
#include "canbus/canopen/nmt_request_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_rpdo1.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h"
#include <cmath> #include <cmath>
namespace cmvr { namespace cmvr {
@ -30,7 +31,7 @@ namespace cmvr {
void setMode(uint8_t node_id, msgs::RunMode mode); 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 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 setQ(uint8_t node_id, double angle_rad) override;
void setLimitQ(uint8_t node_id, double ub, double lb) override; void setLimitQ(uint8_t node_id, double ub, double lb) override;
void setLimitQd(uint8_t node_id, double qd) override; void setLimitQd(uint8_t node_id, double qd) override;
@ -91,6 +92,7 @@ namespace cmvr {
// rpdo1 // rpdo1
std::map<uint8_t, motor::Ti5MotorRPDO1 *> rpdo1_commands_{}; std::map<uint8_t, motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
std::map<uint8_t, motor::Ti5MotorRPDO2 *> rpdo2_commands_{};
void setPPTargetPosBySdo(uint8_t node_id, int32_t pos); void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
@ -102,7 +104,8 @@ namespace cmvr {
void configTPDO1(uint8_t node_id); void configTPDO1(uint8_t node_id);
void configTPDO2(uint8_t node_id); void configTPDO2(uint8_t node_id);
// 目标位置 607A + 控制字 6040 // 目标位置 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<bool()> condition, int timeout_ms) { bool waitUntil(std::function<bool()> condition, int timeout_ms) {