refactor(motor): unify motor command interface
This commit is contained in:
parent
5c8847d334
commit
d292360a8d
@ -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(); }
|
||||
|
||||
@ -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::steady_clock::duration>(
|
||||
std::chrono::duration<double>(dt_segment));
|
||||
|
||||
@ -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<std::mutex> lock(mutex_);
|
||||
sdo_frame_.set_cs(cs);
|
||||
sdo_frame_.set_index(index);
|
||||
|
||||
@ -49,10 +49,10 @@ namespace cmvr {
|
||||
auto command = static_cast<msgs::CommandSpecifier>(bytes[0]);
|
||||
|
||||
// 解析 index（字节1和字节2，低字节优先）
|
||||
auto index = static_cast<msgs::ObIndex>(bytes[1] + (bytes[2] << 8));
|
||||
const uint32_t index = bytes[1] + (bytes[2] << 8);
|
||||
|
||||
// 解析 subindex（字节3）
|
||||
auto subindex = static_cast<msgs::ObSubIndex>(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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<double>& velocities);
|
||||
|
||||
private:
|
||||
bool holdPosition_();
|
||||
double clampQ_(double q) const;
|
||||
double clampQd_(double qd) const;
|
||||
std::shared_ptr<simulate::MujocoWorld> worldLocked_() const;
|
||||
|
||||
@ -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_();
|
||||
|
||||
@ -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<Ti5MotorCanopenProtocol>(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};
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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 <cmath>
|
||||
#include <unordered_map>
|
||||
|
||||
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<AbstractCanbus> can_client_{nullptr};
|
||||
|
||||
// key node_id
|
||||
// std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
|
||||
std::unordered_map<uint8_t,uint32_t> last_Qd_{};
|
||||
std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
|
||||
std::unordered_map<uint8_t, MotorConversion> motor_conversions_{};
|
||||
std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
|
||||
std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
|
||||
|
||||
@ -94,11 +113,7 @@ namespace cmvr {
|
||||
std::map<uint8_t, motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
|
||||
std::map<uint8_t, motor::Ti5MotorRPDO2 *> rpdo2_commands_{};
|
||||
|
||||
void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
|
||||
|
||||
void 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<bool()> condition, int timeout_ms) {
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
|
||||
@ -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<int32_t>(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());
|
||||
}
|
||||
//
|
||||
|
||||
@ -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";
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -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<int>(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<uint32_t>(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<int32_t>(cmd));
|
||||
// break;
|
||||
case RUN_MODE_PROFILE_POSITION:
|
||||
// setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
|
||||
setPPTargetPosBySdo(node_id, static_cast<int32_t>(cmd));
|
||||
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<uint32_t>(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<int32_t>(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<uint32_t>(static_cast<int32_t>(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<int>(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<uint32_t>(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<msgs::RobotDetail>();
|
||||
message_manager_->GetSensorData(data_ptr.get());
|
||||
auto cnt = data_ptr->motors().at(node_id).position();
|
||||
return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG);
|
||||
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<msgs::RobotDetail>();
|
||||
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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user