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"/>
</LeftArmCan>
<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="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="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="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" 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="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="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="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan>
<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) {
auto channel_id = cfg.getAttrDefault("channelId", 0);
auto channel_id = cfg.getAttrDefault("channelId", 1);
port_ = static_cast<CANCardParameter::CANChannelId>(channel_id);
interface_ = CANCardParameter::NATIVE;

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

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

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

@ -119,6 +119,14 @@ namespace cmvr::device{
protocol_->setTarget(node_id_,q, qd);
}
virtual void setTarget(double qd) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
throw std::runtime_error("Protocol not set for motor");
}
protocol_->setTarget(node_id_, qd);
}
virtual bool calibrateZeroQ() {
std::scoped_lock lock(mtx_);
if (!protocol_) {

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

@ -18,7 +18,7 @@ using namespace cmvr::msgs;
TEST(MotorMangerTest,MyTest) {
uint8_t id = 17;
uint8_t id = 22;
XmlNode cfg;
// 1 === 初始化公共组件 ===
@ -65,9 +65,25 @@ TEST(MotorMangerTest,MyTest) {
}
// 6 === 控制电机 ===
auto motor_3 = manager->getMotor("L_SHOULDER_R");
auto motor_3 = manager->getMotor(id);
motor_3->init();
motor_3->calibrateZeroQ();
// motor_3->calibrateZeroQ();
motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY);
motor_3->setTarget(-0.5);
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
motor_3->setTarget(-3.14,1.6);
// std::this_thread::sleep_for(std::chrono::milliseconds(5000));
//
// motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
// motor_3->setTarget(0,1.6);
// motor_3->calibrateZeroQ();
// motor_3->setMode(RUN_MODE_PROFILE_POSITION);
// motor_3->setLimitQ(30.14,-40.14);
// motor_3->setQ(-30);
@ -82,9 +98,14 @@ TEST(MotorMangerTest,MyTest) {
// manager->setMode(id,RUN_MODE_CYCLIC_SYNC_POSITION);
// manager->setQ(id,3.14);
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
auto q = motor_3->getQ();
auto qd = motor_3->getQd();
std::cout << q << ", " << qd << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1));
// motor_3->getQ(q);
}
LOG(INFO) << "Testing MotorManger";

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 setTarget(uint8_t node_id, double angle_rad,double vel) = 0;
virtual void setTarget(uint8_t node_id,double vel) = 0;
virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
virtual msgs::RunMode getMode(uint8_t node_id) = 0;
virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;

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_tpdo2.cpp
${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_rpdo1.cpp
${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_rpdo2.cpp
${CMAKE_CURRENT_SOURCE_DIR}/canopen/ti5_motor_canopen_protocol.cpp
${CMAKE_CURRENT_SOURCE_DIR}/ti5_motor.cpp

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,
std::shared_ptr<MessageManager<msgs::RobotDetail> > manager)
: can_sender_(sender), message_manager_(manager)
{
: can_sender_(sender), message_manager_(manager) {
comm_proto = CommProto::CANOPEN;
// 添加 message
@ -30,8 +29,7 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs:
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!";
}
can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true);
@ -39,15 +37,13 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs:
// sync
sync_command_ = dynamic_cast<SyncProtocol<RobotDetail> *>(
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!";
}
can_sender_->AddMessage(sync_command_->ID, sync_command_, false);
}
bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id)
{
bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
//nmt
message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id);
@ -61,13 +57,13 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id)
//RPDO
message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1, false>(node_id);
message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO2, 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)
{
if (sdo_commands_[node_id] == nullptr) {
LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
}
@ -78,43 +74,48 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id)
rpdo1_commands_[node_id] = dynamic_cast<motor::Ti5MotorRPDO1 *>(
message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO1::ID(node_id)));
if (rpdo1_commands_[node_id] == nullptr)
{
if (rpdo1_commands_[node_id] == nullptr) {
LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
}
can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true);
// rpdo2
rpdo2_commands_[node_id] = dynamic_cast<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;
}
void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index,
uint32_t data, uint32_t delay_ms)
{
uint32_t data, uint32_t delay_ms) {
sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data);
can_sender_->Update(sdo_commands_[node_id]->ID());
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
}
void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad)
{
void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) {
auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
switch (cur_mode_[node_id])
{
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));
// setPPTargetPosBySdo(node_id,static_cast<int32_t>(cmd));
// setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
setPPTargetPosBySdo(node_id, static_cast<int32_t>(cmd));
break;
}
}
void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel)
{
void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) {
auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
rpdo1_commands_[node_id]->SetTargetPos(pos_cmd);
@ -123,8 +124,14 @@ void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, doubl
}
void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos)
{
void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double vel) {
auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
rpdo2_commands_[node_id]->SetTargetVel(int16_t(speed));
can_sender_->Update(rpdo2_commands_[node_id]->ID());
}
void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
controlword_t cw = {};
cw.switch_on = 1;
cw.enable_voltage = 1;
@ -146,8 +153,7 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos)
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
}
void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos)
{
void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
// 触发目标位置运动
controlword_t cw;
cw.value = 0x0F;
@ -165,16 +171,14 @@ void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos)
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos)
{
void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
rpdo1_commands_[node_id]->SetTargetPos(pos);
rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
{
void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
cur_mode_[node_id] = mode;
// 1 : 先设置模式
auto data = static_cast<uint32_t>(mode);
@ -186,6 +190,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
cw.quick_stop = 1;
cw.enable_voltage = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
// configRPDO1(node_id, false);
// configRPDO2(node_id, false);
// 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
@ -194,10 +200,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
switch (mode)
{
case RUN_MODE_PROFILE_POSITION:
{
switch (mode) {
case RUN_MODE_PROFILE_POSITION: {
// 4 : 设置目标位置（为当前位置）
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
@ -214,8 +218,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
break;
}
case RUN_MODE_CYCLIC_SYNC_POSITION:
{
case RUN_MODE_CYCLIC_SYNC_POSITION: {
// configRPDO1(node_id, true);
// 设置目标位置为当前位置
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
@ -227,16 +231,15 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
break;
}
case RUN_MODE_PROFILE_VELOCITY:
{
case RUN_MODE_PROFILE_VELOCITY: {
cw.enable_operation = 1;
cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
case RUN_MODE_CYCLIC_SYNC_VELOCITY:
{
case RUN_MODE_CYCLIC_SYNC_VELOCITY: {
// configRPDO2(node_id, true);
cw.enable_operation = 1;
cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
@ -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);
can_sender_->Update(nmt_command_->ID);
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
}
void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel)
{
void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
}
void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id)
{
void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) {
//TDPO1 配置 状态字 和 控制字
// 1: 失能 pdo
uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
@ -305,8 +305,7 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id)
}
void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id)
{
void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) {
// 1: 失能 pdo
uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
@ -336,8 +335,7 @@ void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id)
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
}
void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start)
{
void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool enable) {
// 1: 失能 pdo
uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
@ -357,45 +355,66 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start)
TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
//6 : 映射控制字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32);
PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32);
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 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);
configTPDO2(node_id);
configRPDO1(node_id, true);
configRPDO2(node_id, true);
}
void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd)
{
void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) {
auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
}
void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd)
{
void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd) {
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
// seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
}
void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb)
{
void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) {
ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0;
lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0;
@ -404,8 +423,7 @@ void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb)
}
bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id)
{
bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
// 0: 设置控制字为 0x06，确保停机状态
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000);
@ -414,11 +432,9 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id)
// 2: 等待确认清除成功
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
if (!waitUntil([&]()
{
if (!waitUntil([&]() {
return GetRobotDetail()->motors().at(node_id).position_offset() == 0;
}, 1000))
{
}, 1000)) {
LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed";
return false;
}
@ -436,11 +452,9 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id)
// 6: 确认写入成功
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
if (!waitUntil([&]()
{
if (!waitUntil([&]() {
return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos;
}, 500))
{
}, 500)) {
return false;
LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed";
}
@ -448,8 +462,7 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id)
return true;
}
void Ti5MotorCanopenProtocol::brake(uint8_t node_id)
{
void Ti5MotorCanopenProtocol::brake(uint8_t node_id) {
// // 开机未使能电机时调用
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
// 6 抱闸 0 ： 立即停机 自由
@ -461,24 +474,19 @@ void Ti5MotorCanopenProtocol::brake(uint8_t node_id)
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
}
bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id)
{
bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) {
statusword_t st{};
st.value = GetRobotDetail()->motors().at(node_id).status_word();
return st.target_reached == 1;
}
void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd)
{
void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) {
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
switch (cur_mode_[node_id])
{
switch (cur_mode_[node_id]) {
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);
if (it == last_Qd_.end() || it->second != speed)
{
if (it == last_Qd_.end() || it->second != speed) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),
0);
last_Qd_[node_id] = speed;
@ -486,8 +494,7 @@ void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd)
break;
}
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);
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);
auto it = last_Qdd_.find(node_id);
if (it == last_Qdd_.end() || it->second != accel)
{
if (it == last_Qdd_.end() || it->second != accel) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel);
last_Qdd_[node_id] = accel;
}
}
void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id)
{
void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
// 0 ： 立即停机 自由
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20);
@ -522,16 +526,14 @@ void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id)
}
double Ti5MotorCanopenProtocol::getQ(uint8_t node_id)
{
double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) {
auto data_ptr = std::make_unique<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 Ti5MotorCanopenProtocol::getQd(uint8_t node_id)
{
double Ti5MotorCanopenProtocol::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();

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