From d292360a8d491f7f31f3ccd377837711ee58f6b6 Mon Sep 17 00:00:00 2001 From: lgv Date: Thu, 9 Jul 2026 14:19:39 +0800 Subject: [PATCH] refactor(motor): unify motor command interface --- .../motor_robot_arm/include/motor_robot_arm.h | 2 +- .../motor_robot_arm/src/motor_robot_arm.cpp | 53 ++- .../canbus/canopen/sdo_request_protocol.h | 2 +- .../canbus/canopen/sdo_response_protocol.h | 6 +- cmvr-es/devices/motor/abstract_motor.h | 93 ++-- .../drivers/mujoco/include/mujoco_motor.h | 19 +- .../motor/drivers/mujoco/src/mujoco_motor.cpp | 90 +++- .../drivers/ti5_canopen/include/ti5_motor.h | 23 +- .../include/ti5_motor_canopen_protocol.h | 61 ++- .../src/protocol/ti5_motor_sdo_response.cpp | 9 +- .../src/protocol/ti5_motor_tpdo2.cpp | 12 - .../src/ti5_motor_canopen_protocol.cpp | 420 +++++++++++------- .../devices/motor/motor_protocol_interface.h | 41 +- 13 files changed, 542 insertions(+), 289 deletions(-) diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index 7a0af759..35471e29 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -73,7 +73,7 @@ public: bool isConnected() const override { return motor_manager_ != nullptr; } Result powerOn() override { return torqueOn(); } Result powerOff() override { return torqueOff(); } - Result brakeRelease() override { return torqueOn(); } + Result brakeRelease() override; Result shutdown() override; Result clearFault() override { return Result::success(); } Result unlockProtectiveStop() override { return Result::success(); } diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 77e8abbc..5a5db3a3 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -196,7 +196,10 @@ Result MotorRobotArm::torqueOn() if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->brake(); + if (!motor->torqueOn()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to torque on motor for joint: " + joint_name); + } } emergency_stopped_ = false; return Result::success(); @@ -209,7 +212,26 @@ Result MotorRobotArm::torqueOff() if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->torqueOff(); + if (!motor->torqueOff() || !motor->brakeRelease()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to torque off motor for joint: " + joint_name); + } + } + return Result::success(); +} + +Result MotorRobotArm::brakeRelease() +{ + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, + "motor not found for joint: " + joint_name); + } + if (!motor->brakeRelease()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to release brake for joint: " + joint_name); + } } return Result::success(); } @@ -238,7 +260,10 @@ Result MotorRobotArm::emergencyStop() if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->brake(); + if (!motor->quickStop()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to quick stop motor for joint: " + joint_name); + } } emergency_stopped_ = true; return Result::success(); @@ -298,7 +323,11 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti } for (std::size_t i = 0; i < motors.size(); ++i) { const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0; - motors[i]->setTarget(sample.position[i], qd); + if (!motors[i]->commandCyclicPosition(sample.position[i], qd)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to command cyclic position for joint: " + + motors[i]->jointName()); + } } if (k + 1 < samples.size()) { const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t @@ -331,7 +360,11 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); } - motor->setTarget(velocity.velocity[i] * speed_scaling_); + if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to command cyclic velocity for joint: " + + joint_names_[i]); + } } } @@ -457,7 +490,11 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target) if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } - motor->setTarget(target.position[i], 0.0); + if (!motor->commandCyclicPosition(target.position[i], 0.0)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to command cyclic position for joint: " + + joint_names_[i]); + } } return Result::success(); } @@ -740,7 +777,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj return false; } for (std::size_t j = 0; j < motors.size(); ++j) { - motors[j]->setTarget(position[j], velocity[j]); + if (!motors[j]->commandCyclicPosition(position[j], velocity[j])) { + return false; + } } next_deadline += std::chrono::duration_cast( std::chrono::duration(dt_segment)); diff --git a/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h b/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h index 69366ef4..6dfd8374 100644 --- a/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h @@ -30,7 +30,7 @@ namespace cmvr { return BASE_ID + sdo_frame_.node_id(); } - void SetFrameData(msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data) { + void SetFrameData(msgs::CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data) { std::lock_guard lock(mutex_); sdo_frame_.set_cs(cs); sdo_frame_.set_index(index); diff --git a/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h b/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h index 0a48af3e..ca78acd4 100644 --- a/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h @@ -49,10 +49,10 @@ namespace cmvr { auto command = static_cast(bytes[0]); // 解析 index（字节1和字节2，低字节优先） - auto index = static_cast(bytes[1] + (bytes[2] << 8)); + const uint32_t index = bytes[1] + (bytes[2] << 8); // 解析 subindex（字节3） - auto subindex = static_cast(bytes[3]); + const uint32_t subindex = bytes[3]; // 根据 command 解析 data（字节4~7） uint32_t data = 0; @@ -94,4 +94,4 @@ namespace cmvr { ParseSdoData(sdo_response_, sensor_data); } } -} \ No newline at end of file +} diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 983ccec7..35eee16d 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -66,13 +66,22 @@ namespace cmvr::device{ return protocol_->getMode(node_id_); } - virtual void torqueOff() { + virtual bool torqueOn() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->torqueOff(node_id_); + return protocol_->torqueOn(node_id_); + } + + virtual bool torqueOff() { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->torqueOff(node_id_); } virtual void setLimitQ(double ub, double lb) { @@ -101,43 +110,69 @@ namespace cmvr::device{ } // virtual void setLimitTau(double tau) = 0; // virtual void setLimitCurrent(double tau) = 0; - virtual void brake() { + virtual bool brakeRelease() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->brake(node_id_); - } - /** - * - * @param q unit : rad - */ - virtual void setQ(double q) { - std::scoped_lock lock(mtx_); - if (!protocol_) { - CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; - } - protocol_->setQ(node_id_, q); + return protocol_->brakeRelease(node_id_); } - virtual void setTarget(double q,double qd) { + virtual bool quickStop() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->setTarget(node_id_,q, qd); + return protocol_->quickStop(node_id_); + } + virtual bool commandProfilePosition(double target_q, + double max_qd = 0.0, + double max_qdd = 0.0) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandProfilePosition(node_id_, target_q, max_qd, max_qdd); } - virtual void setTarget(double qd) { + virtual bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->setTarget(node_id_, qd); + return protocol_->commandProfileVelocity(node_id_, target_qd, max_qdd); + } + + virtual bool commandCyclicPosition(double target_q, + double target_qd = 0.0) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicPosition(node_id_, target_q, target_qd); + } + + virtual bool commandCyclicVelocity(double target_qd) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicVelocity(node_id_, target_qd); + } + + virtual bool commandCyclicTorque(double target_tau) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicTorque(node_id_, target_tau); } virtual bool calibrateZeroQ() { @@ -156,16 +191,6 @@ namespace cmvr::device{ } return protocol_->reachedTargetQ(node_id_); } - // rad /s - virtual void setQd(double qd) { - std::scoped_lock lock(mtx_); - if (!protocol_) { - CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; - } - return protocol_->setQd(node_id_,qd); - } - // virtual void setQdd(double qdd) = 0; // rad /s^2 // virtual void setTau(double tau) = 0; // N m // virtual void clear_err() = 0; // virtual void getStatus() = 0; diff --git a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h index b0293806..5eb7b5e3 100644 --- a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h +++ b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h @@ -23,19 +23,25 @@ public: void setMode(msgs::RunMode mode) override; msgs::RunMode getMode() override; - void torqueOff() override; + bool torqueOn() override; + bool torqueOff() override; + bool brakeRelease() override; + bool quickStop() override; void setLimitQ(double ub, double lb) override; void setLimitQd(double qd) override; void setLimitQdd(double u_qdd, double l_qdd) override; - void brake() override; - void setQ(double q) override; - void setTarget(double q, double qd) override; - void setTarget(double qd) override; bool calibrateZeroQ() override; bool reachedTargetQ() override; - void setQd(double qd) override; + bool commandProfilePosition(double target_q, + double max_qd = 0.0, + double max_qdd = 0.0) override; + bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) override; + bool commandCyclicPosition(double target_q, + double target_qd = 0.0) override; + bool commandCyclicVelocity(double target_qd) override; + bool commandCyclicTorque(double target_tau) override; double getQ() override; double getQd() override; @@ -44,6 +50,7 @@ public: const std::vector& velocities); private: + bool holdPosition_(); double clampQ_(double q) const; double clampQd_(double qd) const; std::shared_ptr worldLocked_() const; diff --git a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp index 160c1005..07b0bda4 100644 --- a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp +++ b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp @@ -54,11 +54,27 @@ msgs::RunMode MujocoMotor::getMode() return mode_; } -void MujocoMotor::torqueOff() +bool MujocoMotor::torqueOn() { - brake(); + return holdPosition_(); +} + +bool MujocoMotor::torqueOff() +{ + const bool ok = holdPosition_(); std::scoped_lock lock(mtx_); mode_ = msgs::RUN_MODE_UNSPECIFIED; + return ok; +} + +bool MujocoMotor::brakeRelease() +{ + return true; +} + +bool MujocoMotor::quickStop() +{ + return holdPosition_(); } void MujocoMotor::setLimitQ(const double ub, const double lb) @@ -82,38 +98,78 @@ void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd) info_.limit_qdd = std::max(limit_qdd_upper_, std::abs(limit_qdd_lower_)); } -void MujocoMotor::brake() +bool MujocoMotor::holdPosition_() { const auto world = worldLocked_(); double q = 0.0; if (!world || !world->getJointPosition(info_.joint_name, q)) { - return; + return false; } std::scoped_lock lock(mtx_); target_q_ = q; mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; world->setJointTargetState(info_.joint_name, q, 0.0); + return true; } -void MujocoMotor::setQ(const double q) +bool MujocoMotor::commandProfilePosition(const double target_q, + const double max_qd, + const double max_qdd) { - setTarget(q, 0.0); + (void)max_qdd; + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_PROFILE_POSITION; + target_q_ = clampQ_(target_q); + const double profile_qd = max_qd > 0.0 ? max_qd : info_.limit_qd; + return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(profile_qd)); } -void MujocoMotor::setTarget(const double q, const double qd) +bool MujocoMotor::commandProfileVelocity(const double target_qd, const double max_qdd) +{ + (void)max_qdd; + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_PROFILE_VELOCITY; + return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd)); +} + +bool MujocoMotor::commandCyclicPosition(const double target_q, + const double target_qd) { std::scoped_lock lock(mtx_); const auto world = worldLocked_(); if (!world) { - return; + return false; } - target_q_ = clampQ_(q); - world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd)); + mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + target_q_ = clampQ_(target_q); + return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(target_qd)); } -void MujocoMotor::setTarget(const double qd) +bool MujocoMotor::commandCyclicVelocity(const double target_qd) { - setQd(qd); + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd)); +} + +bool MujocoMotor::commandCyclicTorque(const double target_tau) +{ + (void)target_tau; + CMVR_LOG(ERROR) << "[MujocoMotor] cyclic torque command is not implemented: " + << info_.joint_name; + return false; } bool MujocoMotor::calibrateZeroQ() @@ -140,16 +196,6 @@ bool MujocoMotor::reachedTargetQ() } } -void MujocoMotor::setQd(const double qd) -{ - std::scoped_lock lock(mtx_); - const auto world = worldLocked_(); - if (!world) { - return; - } - world->setJointTargetVelocity(info_.joint_name, clampQd_(qd)); -} - double MujocoMotor::getQ() { const auto world = worldLocked_(); diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h index 64db8ea2..d1fc84ae 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h @@ -22,6 +22,8 @@ namespace cmvr { info_.limit_q_ub = config.limit_q_ub(); info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5; info_.limit_qdd = config.limit_qdd() > 0.0 ? config.limit_qdd() : 10.0; + encoder_counts_per_rev_ = config.encoder_counts_per_rev(); + gear_ratio_ = config.gear_ratio(); node_id_ = info_.id; } @@ -37,6 +39,19 @@ namespace cmvr { } if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) { auto canopen_protocol = std::dynamic_pointer_cast(protocol_); + if (!canopen_protocol) { + CMVR_LOG(ERROR) << "[Ti5Motor] invalid CANopen protocol for motor: " + << info_.joint_name; + return false; + } + if (encoder_counts_per_rev_ <= 0.0 || gear_ratio_ <= 0.0) { + CMVR_LOG(ERROR) << "[Ti5Motor] missing encoder conversion config: " + << info_.joint_name + << ", encoder_counts_per_rev=" << encoder_counts_per_rev_ + << ", gear_ratio=" << gear_ratio_; + return false; + } + protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_); // torqueOff(node_id_); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION); // canopen_protocol->torqueOff(node_id_); @@ -45,8 +60,8 @@ namespace cmvr { canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE); canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); - // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb); canopen_protocol->setLimitQd(node_id_, info_.limit_qd); canopen_protocol->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd); @@ -55,6 +70,10 @@ namespace cmvr { } return true; } + + private: + double encoder_counts_per_rev_{0.0}; + double gear_ratio_{0.0}; }; diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h index e837cd89..0588fd3f 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h @@ -18,6 +18,7 @@ #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h" #include +#include namespace cmvr { namespace device { @@ -29,28 +30,40 @@ namespace cmvr { bool initNode(uint8_t node_id) override; - 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 setMode(uint8_t node_id, msgs::RunMode mode) override; void setLimitQ(uint8_t node_id, double ub, double lb) override; void setLimitQd(uint8_t node_id, double qd) override; void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override; bool calibrateZeroQ(uint8_t node_id) override; - void brake(uint8_t node_id) override; + bool torqueOn(uint8_t node_id) override; + bool torqueOff(uint8_t node_id) override; + bool brakeRelease(uint8_t node_id) override; + bool quickStop(uint8_t node_id) override; bool reachedTargetQ(uint8_t node_id) override; double getQ(uint8_t node_id) override; double getQd(uint8_t node_id) override; - void setQd(uint8_t node_id, double qd) override; - void setQdd(uint8_t node_id, double qdd) override; - - void torqueOff(uint8_t node_id) override; + bool commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) override; + bool commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) override; + bool commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) override; + bool commandCyclicVelocity(uint8_t node_id, + double target_qd) override; + bool commandCyclicTorque(uint8_t node_id, double target_tau) override; + void setMotorConversion(uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) override; void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10); - void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index, - msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10); + void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, uint32_t index, + uint32_t sub_index, uint32_t data, uint32_t delay_ms = 10); void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel); void configPdo(uint8_t node_id); @@ -70,14 +83,20 @@ namespace cmvr { private: - static constexpr double GearRatio = 101.0; // 电机减速比 static constexpr double RADTODEG = 180.0 / M_PI; + static constexpr double Ti5VelocityUnitScale = 100.0; + static constexpr double Ti5AccelerationTimeScale = 1000.0; + + struct MotorConversion { + double encoder_counts_per_rev{0.0}; + double gear_ratio{0.0}; + }; + std::shared_ptr can_client_{nullptr}; // key node_id // std::unordered_map cur_mode_{}; - std::unordered_map last_Qd_{}; - std::unordered_map last_Qdd_{}; + std::unordered_map motor_conversions_{}; std::shared_ptr > can_sender_{nullptr}; std::shared_ptr > message_manager_{nullptr}; @@ -94,11 +113,7 @@ namespace cmvr { std::map rpdo1_commands_{}; std::map rpdo2_commands_{}; - void setPPTargetPosBySdo(uint8_t node_id, int32_t pos); - - void setPPTargetPosByPdo(uint8_t node_id, int32_t pos); - - void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos); + void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos); void configTPDO1(uint8_t node_id); @@ -107,6 +122,14 @@ namespace cmvr { void configRPDO1(uint8_t node_id, bool enable); void configRPDO2(uint8_t node_id, bool enable); + const MotorConversion* conversionForNode(uint8_t node_id) const; + double radToCounts(double angle_rad, const MotorConversion& conversion) const; + double countsToRad(int32_t counts, const MotorConversion& conversion) const; + double radPerSecToVelocityRaw(double velocity_rad_s, const MotorConversion& conversion) const; + uint32_t radPerSec2ToAccelerationRaw(double acceleration_rad_s2, + const MotorConversion& conversion) const; + double velocityRawToRadPerSec(int32_t velocity_raw, const MotorConversion& conversion) const; + bool waitUntil(std::function condition, int timeout_ms) { auto start = std::chrono::steady_clock::now(); diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp index b5486dec..4410332d 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp @@ -3,6 +3,7 @@ // Created by lgv on 2025/7/24. // +#include "cmvr/msgs/cia402.pb.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h" using namespace cmvr::device::motor; @@ -19,17 +20,17 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, switch (sdo_response.index()) { - case msgs::CONTROL_WORD_6040: + case msgs::CIA402_CONTROL_WORD_6040: motor_status->set_ctrl_word(sdo_response.data()); break; - case msgs::STATUS_WORD_6041: + case msgs::CIA402_STATUS_WORD_6041: motor_status->set_status_word(sdo_response.data()); break; - case msgs::ACTUAL_POSITION_6064: + case msgs::CIA402_ACTUAL_POSITION_6064: motor_status->set_position(static_cast(sdo_response.data())); CMVR_LOG(INFO) << "pos = " << motor_status->position(); break; - case msgs::POSITION_OFFSET_2008: + case msgs::CANOPEN_POSITION_OFFSET_2008: motor_status->set_position_offset(sdo_response.data()); } // diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp index e4c98b36..d1f67403 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp @@ -19,16 +19,4 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]); motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]); - - - double gearRatio = 101.0; - double radToDeg = 180.0 / M_PI; - auto speed = (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0); - - auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg); - - // CMVR_LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s"; - - - } diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp index c5546b2a..69e1f562 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp @@ -3,6 +3,7 @@ // Created by lgv on 2025/8/1. // +#include "cmvr/msgs/cia402.pb.h" #include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" #include "canbus/canopen/register.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h" @@ -94,45 +95,153 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { return ErrorCode::OK; } +void Ti5MotorCanopenProtocol::setMotorConversion( + const uint8_t node_id, + const double encoder_counts_per_rev, + const double gear_ratio) { + motor_conversions_[node_id] = {encoder_counts_per_rev, gear_ratio}; +} -void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index, +const Ti5MotorCanopenProtocol::MotorConversion* +Ti5MotorCanopenProtocol::conversionForNode(const uint8_t node_id) const { + const auto it = motor_conversions_.find(node_id); + if (it != motor_conversions_.end() && + it->second.encoder_counts_per_rev > 0.0 && + it->second.gear_ratio > 0.0) { + return &it->second; + } + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] missing conversion config for node " + << static_cast(node_id); + return nullptr; +} + +double Ti5MotorCanopenProtocol::radToCounts( + const double angle_rad, + const MotorConversion& conversion) const { + return (angle_rad * RADTODEG) / 360.0 * + conversion.gear_ratio * conversion.encoder_counts_per_rev; +} + +double Ti5MotorCanopenProtocol::countsToRad( + const int32_t counts, + const MotorConversion& conversion) const { + return (counts * 360.0) / + (conversion.gear_ratio * conversion.encoder_counts_per_rev * RADTODEG); +} + +double Ti5MotorCanopenProtocol::radPerSecToVelocityRaw( + const double velocity_rad_s, + const MotorConversion& conversion) const { + return ((velocity_rad_s * RADTODEG) * conversion.gear_ratio * Ti5VelocityUnitScale) / + 360.0; +} + +uint32_t Ti5MotorCanopenProtocol::radPerSec2ToAccelerationRaw( + const double acceleration_rad_s2, + const MotorConversion& conversion) const { + const auto raw = ((std::abs(acceleration_rad_s2) * RADTODEG) * + conversion.gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + return static_cast(std::abs(raw)); +} + +double Ti5MotorCanopenProtocol::velocityRawToRadPerSec( + const int32_t velocity_raw, + const MotorConversion& conversion) const { + return (velocity_raw * 360.0) / + (conversion.gear_ratio * Ti5VelocityUnitScale * RADTODEG); +} + + +void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data, uint32_t delay_ms) { sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data); can_sender_->Update(sdo_commands_[node_id]->ID()); std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); } -void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) { - auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; - - switch (getMode(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; +bool Ti5MotorCanopenProtocol::commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; } + if (max_qd > 0.0) { + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, + SUB_INDEX_0, + static_cast(std::abs(radPerSecToVelocityRaw(max_qd, *conversion))), + 0); + } + if (max_qdd > 0.0) { + const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, + SUB_INDEX_0, accel, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, + SUB_INDEX_0, accel, 0); + } + writeProfilePositionTargetBySdo(node_id, static_cast(radToCounts(target_q, *conversion))); + return true; } -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; +bool Ti5MotorCanopenProtocol::commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + if (max_qdd > 0.0) { + const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, + SUB_INDEX_0, accel, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, + SUB_INDEX_0, accel, 0); + } + const auto speed = radPerSecToVelocityRaw(target_qd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_VELOCITY_60FF, + SUB_INDEX_0, + static_cast(static_cast(std::llround(speed))), + 0); + return true; +} + +bool Ti5MotorCanopenProtocol::commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + auto pos_cmd = radToCounts(target_q, *conversion); + auto speed = radPerSecToVelocityRaw(target_qd, *conversion); rpdo1_commands_[node_id]->SetTargetPos(pos_cmd); rpdo1_commands_[node_id]->SetTargetVel(uint32_t(std::abs(speed))); can_sender_->Update(rpdo1_commands_[node_id]->ID()); + return true; } - -void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double vel) { - auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; +bool Ti5MotorCanopenProtocol::commandCyclicVelocity(uint8_t node_id, + double target_qd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + auto speed = radPerSecToVelocityRaw(target_qd, *conversion); rpdo2_commands_[node_id]->SetTargetVel(int16_t(speed)); can_sender_->Update(rpdo2_commands_[node_id]->ID()); + return true; } +bool Ti5MotorCanopenProtocol::commandCyclicTorque(uint8_t node_id, double target_tau) { + (void)target_tau; + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] cyclic torque command is not implemented, node=" + << static_cast(node_id); + return false; +} -void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) { +void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos) { controlword_t cw = {}; cw.switch_on = 1; cw.enable_voltage = 1; @@ -141,44 +250,19 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) cw.change_set_immediately = 1; // 1. 设置目标位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, pos); // 2. 设置触发位（bit4 = 1） cw.new_set_point = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); // 3. 清除触发位（bit4 = 0），准备下一次触发 cw.new_set_point = 0; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); } -void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) { - // 触发目标位置运动 - controlword_t cw; - cw.value = 0x0F; - cw.new_set_point = 1; - cw.change_set_immediately = 1; - - rpdo1_commands_[node_id]->SetTargetPos(pos); - rpdo1_commands_[node_id]->SetCtrlWord(cw.value); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - cw.new_set_point = 0; - rpdo1_commands_[node_id]->SetCtrlWord(cw.value); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); -} - -void 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) { // cur_mode_[node_id] = mode; @@ -188,36 +272,36 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { controlword_t cw = {}; cw.quick_stop = 1; cw.enable_voltage = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); // configRPDO1(node_id, false); // configRPDO2(node_id, false); // 1 : 先设置模式 auto data = static_cast(mode); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CIA402_OPERATION_MODE_6060, SUB_INDEX_0, data); // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F） cw.switch_on = 1; cw.enable_operation = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); switch (mode) { case RUN_MODE_PROFILE_POSITION: { // 4 : 设置目标位置（为当前位置） auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); // 5 : 触发位置运动（new_set_point 翻转） cw.new_set_point = 1; cw.change_set_immediately = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); // 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标） cw.new_set_point = 0; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } @@ -225,19 +309,19 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // 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); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); //3 : 使能 15 cw.enable_operation = 1; cw.switch_on = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } case RUN_MODE_PROFILE_VELOCITY: { cw.enable_operation = 1; cw.switch_on = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } @@ -245,7 +329,7 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // 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); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } default: @@ -262,9 +346,9 @@ void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand c 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); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, SUB_INDEX_0, speed); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); } @@ -272,128 +356,128 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) { //TDPO1 配置 状态字 和 控制字 // 1: 失能 pdo uint32_t cob_id = TPDO1_BASE_ID_180 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 0); // 2: 配置为异步 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // 3：配置约束时间 unit:0.1ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_3, 10); // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_5, 0); // 5 ：映射控制字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1, - CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_1, + CIA402_CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); //6 : 映射状态字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2, - STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_2, + CIA402_STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16); //7 : 映射模式 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3, - MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_3, + CIA402_MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8); //8 映射错误码 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4, - ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_4, + CIA402_ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16); //9 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 4); //10 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31)); } 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)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 0); // 2: 配置为异步 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // 3：配置约束时间 unit:0.1ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_3, 100); // 4 : 配置周期发送时间 unit : ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_5, 0); // 5 ：映射当前位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1, - ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_1, + CIA402_ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32); //6 : 映射当前速度 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2, - ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_2, + CIA402_ACTUAL_VELOCITY_606C << 16 | SUB_INDEX_0 << 8 | 32); //9 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 2); //10 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31)); } 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)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 0); // 2: 配置为 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // // 3：配置约束时间 unit:0.1ms - // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10); + // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_3,10); // // // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 - // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0); + // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_5,0); // 5 ：映射位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1, - TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_1, + CIA402_TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); //6 : 映射控制字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2, - PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_2, + CIA402_PROFILE_VELOCITY_6081 << 16 | SUB_INDEX_0 << 8 | 32); //7 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_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)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_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); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_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); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_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); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_1, + CIA402_TARGET_VELOCITY_60FF << 16 | SUB_INDEX_0 << 8 | 32); //7 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 1); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_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)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31)); } @@ -405,36 +489,49 @@ void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) { } 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)); + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + auto accel = ((u_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + auto decel = ((l_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel)); } 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); + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + auto speed = radPerSecToVelocityRaw(qd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_MAX_PROFILE_VELOCITY_607F, 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; + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + ub = radToCounts(ub, *conversion); + lb = radToCounts(lb, *conversion); - 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, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub); } 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, CIA402_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); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0); // 2: 等待确认清除成功 - seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0); if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == 0; }, 1000)) { @@ -443,18 +540,18 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { } // 3: 读取当前位置 0x6064 - seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); + seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); // 4: 将当前位置写入偏置寄存器 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_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, CANOPEN_USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100); // 6: 确认写入成功 - seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); + seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos; }, 500)) { @@ -465,16 +562,28 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { return true; } -void Ti5MotorCanopenProtocol::brake(uint8_t node_id) { +bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) { + setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + return true; +} + +bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) { + (void)node_id; + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented"; + return false; +} + +bool Ti5MotorCanopenProtocol::quickStop(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, CIA402_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_FOUR_BYTES, CIA402_QUICK_STOP_DECELERATION_6085, SUB_INDEX_0, 0XFFFFFFF0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100); // 必须要发送 0xf 才能按照6085中设定的减速度减速 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + return true; } bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { @@ -483,62 +592,37 @@ bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { return st.target_reached == 1; } -void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) { - auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; - switch (getMode(node_id)) { - case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: - case msgs::RUN_MODE_PROFILE_POSITION: { - auto it = last_Qd_.find(node_id); - if (it == last_Qd_.end() || it->second != speed) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)), - 0); - last_Qd_[node_id] = speed; - } - break; - } - case msgs::RUN_MODE_PROFILE_VELOCITY: - 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) { - uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0); - auto it = last_Qdd_.find(node_id); - if (it == last_Qdd_.end() || it->second != accel) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel); - last_Qdd_[node_id] = accel; - } -} - -void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) { +bool 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, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20); // 必须要发送 0xf 才能按照6085中设定的减速度减速 - // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); // 停机之后，要重新使能？ // cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED; + return true; } double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return 0.0; + } 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); + return countsToRad(cnt, *conversion); } double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return 0.0; + } 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); + return velocityRawToRadPerSec(cnt, *conversion); } diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 634a715c..4b8f3e34 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -15,7 +15,8 @@ namespace cmvr { public: enum class CommProto : uint8_t { CANOPEN = 1, - CUSTOM = 2 + ETHERCAT = 2, + CUSTOM = 3 }; virtual ~MotorProtocolInterface() = default; @@ -26,9 +27,6 @@ namespace cmvr { */ virtual bool initNode(uint8_t node_id) = 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 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; @@ -36,12 +34,35 @@ namespace cmvr { virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0; virtual bool calibrateZeroQ(uint8_t node_id) = 0; virtual bool reachedTargetQ(uint8_t node_id) = 0; - virtual void setQd(uint8_t node_id, double qd) = 0; - virtual void setQdd(uint8_t node_id,double qdd) = 0; - // virtual void setVelocity(uint8_t node_id, double velocity) = 0; - // virtual void clearError(uint8_t node_id) = 0; - virtual void brake(uint8_t node_id) = 0; - virtual void torqueOff(uint8_t node_id) = 0; + // target_q: rad, max_qd: rad/s, max_qdd: rad/s^2. + // Profile Position 写入目标位置和轮廓速度/加速度，并触发一次新目标。 + virtual bool commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) = 0; + // target_qd: rad/s, max_qdd: rad/s^2. + // Profile Velocity 写入目标速度和轮廓加速度。 + virtual bool commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) = 0; + // target_q: rad, target_qd: rad/s. + // Cyclic Position 周期写入目标位置和目标速度。 + virtual bool commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) = 0; + // target_qd: rad/s. + // Cyclic Velocity 周期写入目标速度。 + virtual bool commandCyclicVelocity(uint8_t node_id, + double target_qd) = 0; + // target_tau: N*m. + virtual bool commandCyclicTorque(uint8_t node_id, double target_tau) = 0; + virtual void setMotorConversion(uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) = 0; + virtual bool torqueOn(uint8_t node_id) = 0; + virtual bool torqueOff(uint8_t node_id) = 0; + virtual bool brakeRelease(uint8_t node_id) = 0; + virtual bool quickStop(uint8_t node_id) = 0; virtual double getQ(uint8_t node_id) = 0; virtual double getQd(uint8_t node_id) = 0;