From e06f8f9f1af31269b121b49a6d110eded3ae075a Mon Sep 17 00:00:00 2001 From: lgv Date: Tue, 3 Mar 2026 14:06:26 +0800 Subject: [PATCH] feat : add cyclic synchronous speed mode for ti5 motor --- cmvr-es/devices/motor/abstract_motor.h | 4 +- .../devices/motor/motor_protocol_interface.h | 2 +- .../canopen/protocol/ti5_motor_rpdo1.cpp | 12 +- .../canopen/protocol/ti5_motor_rpdo1.h | 2 +- .../canopen/ti5_motor_canopen_protocol.cpp | 232 +++++++++++------- .../canopen/ti5_motor_canopen_protocol.h | 2 +- 6 files changed, 150 insertions(+), 104 deletions(-) diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index d5eba11e..3f356cc8 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -111,12 +111,12 @@ namespace cmvr::device{ protocol_->setQ(node_id_, q); } - virtual void set(double q,double qd) { + virtual void setTarget(double q,double qd) { std::scoped_lock lock(mtx_); if (!protocol_) { throw std::runtime_error("Protocol not set for motor"); } - protocol_->set(node_id_,q, qd); + protocol_->setTarget(node_id_,q, qd); } virtual bool calibrateZeroQ() { diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 9d949f7c..0ada73af 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -27,7 +27,7 @@ namespace cmvr { virtual bool initNode(uint8_t node_id) = 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 msgs::RunMode getMode(uint8_t node_id) = 0; virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp index f8c0ffaf..88c655a2 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp @@ -16,11 +16,11 @@ void Ti5MotorRPDO1::UpdateData(uint8_t *data) { data[2] = target_pos_ >> 16 & 0xFF; data[3] = target_pos_ >> 24 & 0xFF; // - data[4] = ctrl_word_ & 0xFF; - data[5] = ctrl_word_ >> 8 & 0xFF; + // data[4] = ctrl_word_ & 0xFF; + // data[5] = ctrl_word_ >> 8 & 0xFF; - // data[4] = target_vel_ & 0xFF; - // data[5] = target_vel_ >> 8 & 0xFF; - // data[6] = target_vel_ >> 16 & 0xFF; - // data[7] = target_vel_ >> 24 & 0xFF; + data[4] = target_vel_ & 0xFF; + data[5] = target_vel_ >> 8 & 0xFF; + data[6] = target_vel_ >> 16 & 0xFF; + data[7] = target_vel_ >> 24 & 0xFF; } diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h index de387cc0..df943618 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h @@ -25,7 +25,7 @@ namespace cmvr { void UpdateData(uint8_t *data) override; int32_t GetLength() const override { - return 0x06; + return 0x08; } uint32_t GetPeriod() const override { diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp index 82706b9c..e83bbaa8 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp @@ -11,10 +11,10 @@ using namespace cmvr::device; using namespace cmvr::msgs; -Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr > sender, - std::shared_ptr > manager) - : can_sender_(sender), message_manager_(manager){ - +Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr> sender, + std::shared_ptr> manager) + : can_sender_(sender), message_manager_(manager) +{ comm_proto = CommProto::CANOPEN; // 添加 message @@ -26,25 +26,28 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr *>( + nmt_command_ = dynamic_cast*>( message_manager_->GetMutableProtocolDataById(NmtRequestProtocol::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); // sync - sync_command_ = dynamic_cast *>( + sync_command_ = dynamic_cast*>( message_manager_->GetMutableProtocolDataById(SyncProtocol::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, false>(node_id); @@ -53,19 +56,18 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { message_manager_->AddRecvProtocolData(node_id); //TPDO - message_manager_->AddRecvProtocolData(node_id); - message_manager_->AddRecvProtocolData(node_id); + message_manager_->AddRecvProtocolData(node_id); + message_manager_->AddRecvProtocolData(node_id); //RPDO - message_manager_->AddSendProtocolData(node_id); + message_manager_->AddSendProtocolData(node_id); - - - sdo_commands_[node_id] = dynamic_cast *>( + sdo_commands_[node_id] = dynamic_cast*>( message_manager_->GetMutableProtocolDataById(SdoRequestProtocol::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; } @@ -73,10 +75,11 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { // pdo1 - rpdo1_commands_[node_id] = dynamic_cast( + rpdo1_commands_[node_id] = dynamic_cast( 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; } @@ -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, - 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]) { - case RUN_MODE_CYCLIC_SYNC_POSITION: - setCSPTargetPosByPdo(node_id, static_cast(cmd)); - break; - case RUN_MODE_PROFILE_POSITION: - setPPTargetPosByPdo(node_id, static_cast(cmd)); - // setPPTargetPosBySdo(node_id,static_cast(cmd)); - break; + switch (cur_mode_[node_id]) + { + case RUN_MODE_CYCLIC_SYNC_POSITION: + setCSPTargetPosByPdo(node_id, static_cast(cmd)); + break; + case RUN_MODE_PROFILE_POSITION: + setPPTargetPosByPdo(node_id, static_cast(cmd)); + // setPPTargetPosBySdo(node_id,static_cast(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 speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; 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 = {}; cw.switch_on = 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); } -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; @@ -156,15 +165,16 @@ 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(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); - 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); @@ -202,7 +214,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { break; } - case RUN_MODE_CYCLIC_SYNC_POSITION: { + 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); @@ -214,33 +227,45 @@ 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; } - 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; + } + 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); 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; @@ -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 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)); @@ -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)); } -void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) { +void Ti5MotorCanopenProtocol::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)); @@ -331,10 +358,11 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) { //6 : 映射控制字 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映射对象总个数 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); configTPDO2(node_id); 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 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, 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) { - ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0; - lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0; +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; 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 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); + 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([&]() { + if (!waitUntil([&]() + { return GetRobotDetail()->motors().at(node_id).position_offset() == 0; - }, 1000)) { + }, 1000)) + { LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed"; 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); // 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: 确认写入成功 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"; } @@ -412,61 +448,72 @@ 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 ： 立即停机 自由 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); + 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 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 ; + 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]) { - case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: - case msgs::RUN_MODE_PROFILE_POSITION: { + 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); + 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: + 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; + } + 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); 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); + 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); @@ -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(); message_manager_->GetSensorData(data_ptr.get()); - auto cnt = data_ptr->motors().at(node_id).position(); - return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG); + 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(); message_manager_->GetSensorData(data_ptr.get()); - auto cnt = data_ptr->motors().at(node_id).speed(); - return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG); + auto cnt = data_ptr->motors().at(node_id).speed(); + return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG); } - - - diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h index 09a9b955..8a0a7e80 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h @@ -29,7 +29,7 @@ namespace cmvr { bool initNode(uint8_t node_id) override; 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 setLimitQ(uint8_t node_id, double ub, double lb) override;