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

feat : add cyclic synchronous speed mode for ti5 motor

Browse Source
This commit is contained in:
lgv 2026-03-03 14:06:26 +08:00
parent 441a5e6e64
commit e06f8f9f1a
6 changed files with 150 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -27,7 +27,7 @@ namespace cmvr {
virtual bool initNode(uint8_t node_id) = 0; virtual bool initNode(uint8_t node_id) = 0;
virtual void setQ(uint8_t node_id, double angle_rad) = 0; virtual void setQ(uint8_t node_id, double angle_rad) = 0;
virtual void set(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 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;

12
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp
View File

@ -16,11 +16,11 @@ void Ti5MotorRPDO1::UpdateData(uint8_t *data) {
data[2] = target_pos_ >> 16 & 0xFF; data[2] = target_pos_ >> 16 & 0xFF;
data[3] = target_pos_ >> 24 & 0xFF; data[3] = target_pos_ >> 24 & 0xFF;
// //
data[4] = ctrl_word_ & 0xFF; // data[4] = ctrl_word_ & 0xFF;
data[5] = ctrl_word_ >> 8 & 0xFF; // data[5] = ctrl_word_ >> 8 & 0xFF;
// data[4] = target_vel_ & 0xFF; data[4] = target_vel_ & 0xFF;
// data[5] = target_vel_ >> 8 & 0xFF; data[5] = target_vel_ >> 8 & 0xFF;
// data[6] = target_vel_ >> 16 & 0xFF; data[6] = target_vel_ >> 16 & 0xFF;
// data[7] = target_vel_ >> 24 & 0xFF; data[7] = target_vel_ >> 24 & 0xFF;
} }

2
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h
View File

@ -25,7 +25,7 @@ namespace cmvr {
void UpdateData(uint8_t *data) override; void UpdateData(uint8_t *data) override;
int32_t GetLength() const override { int32_t GetLength() const override {
return 0x06; return 0x08;
} }
uint32_t GetPeriod() const override { uint32_t GetPeriod() const override {

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

@ -11,10 +11,10 @@
using namespace cmvr::device; using namespace cmvr::device;
using namespace cmvr::msgs; 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
@ -26,25 +26,28 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs:
// nmt // nmt
nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail> *>( nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail>*>(
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);
// 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);
@ -53,19 +56,18 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
message_manager_->AddRecvProtocolData<motor::Ti5MotorSdoResponse, false>(node_id); message_manager_->AddRecvProtocolData<motor::Ti5MotorSdoResponse, false>(node_id);
//TPDO //TPDO
message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO1,false>(node_id); message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO1, false>(node_id);
message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO2,false>(node_id); message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO2, false>(node_id);
//RPDO //RPDO
message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1,false>(node_id); message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1, 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;
} }
@ -73,10 +75,11 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
// pdo1 // pdo1
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;
} }
@ -87,27 +90,31 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
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: {
setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd)); case RUN_MODE_CYCLIC_SYNC_POSITION:
break; setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
case RUN_MODE_PROFILE_POSITION: break;
setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd)); case RUN_MODE_PROFILE_POSITION:
// setPPTargetPosBySdo(node_id,static_cast<int32_t>(cmd)); setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
break; // setPPTargetPosBySdo(node_id,static_cast<int32_t>(cmd));
break;
} }
} }
void Ti5MotorCanopenProtocol::set(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);
@ -116,7 +123,8 @@ void Ti5MotorCanopenProtocol::set(uint8_t node_id, double angle_rad, double vel)
} }
void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) { 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;
@ -138,7 +146,8 @@ 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;
@ -156,15 +165,16 @@ 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);
@ -184,8 +194,10 @@ 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);
@ -202,7 +214,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:
{
// 设置目标位置为当前位置 // 设置目标位置为当前位置
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,33 +227,45 @@ 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;
} }
default:
// TODO: Handle unspecified or unknown mode case RUN_MODE_CYCLIC_SYNC_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; break;
}
default:
// TODO: Handle unspecified or unknown mode
break;
} }
} }
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;
@ -280,7 +305,8 @@ 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));
@ -310,7 +336,8 @@ 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 start)
{
// 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));
@ -331,10 +358,11 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) {
//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,
CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32);
if (start) { 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);
@ -344,47 +372,53 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) {
} }
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);
} }
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);
} }
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; 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_1, lb);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
} }
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);
// 1: 清除偏置值 0x2008 ← 0 // 1: 清除偏置值 0x2008 ← 0
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
// 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;
} }
@ -397,14 +431,16 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
// 5: 保存参数到永久区（0x2000 ← 1） // 5: 保存参数到永久区（0x2000 ← 1）
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1,100); seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100);
// 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";
} }
@ -412,61 +448,72 @@ 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 ： 立即停机 自由
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0); 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, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02,100); seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100);
// 必须要发送 0xf 才能按照6085中设定的减速度减速 // 必须要发送 0xf 才能按照6085中设定的减速度减速
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_PROFILE_POSITION: { case msgs::RUN_MODE_CYCLIC_SYNC_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)),0); {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),
0);
last_Qd_[node_id] = speed; last_Qd_[node_id] = speed;
} }
break; break;
} }
case msgs::RUN_MODE_PROFILE_VELOCITY: { case msgs::RUN_MODE_PROFILE_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; // 在速度模式下，直接设置目标速度
} seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
default:
break; break;
}
default:
break;
} }
} }
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);
// 必须要发送 0xf 才能按照6085中设定的减速度减速 // 必须要发送 0xf 才能按照6085中设定的减速度减速
// 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);
@ -475,19 +522,18 @@ 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();
return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG); return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
} }

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

@ -29,7 +29,7 @@ namespace cmvr {
bool initNode(uint8_t node_id) override; bool initNode(uint8_t node_id) override;
void setMode(uint8_t node_id, msgs::RunMode mode); void setMode(uint8_t node_id, msgs::RunMode mode);
void set(uint8_t node_id, double angle_rad, double vel) override; void setTarget(uint8_t node_id, double angle_rad, 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;