feat : add cyclic synchronous speed mode for ti5 motor
This commit is contained in:
parent
441a5e6e64
commit
e06f8f9f1a
@ -111,12 +111,12 @@ namespace cmvr::device{
|
||||
protocol_->setQ(node_id_, q);
|
||||
}
|
||||
|
||||
virtual void set(double q,double qd) {
|
||||
virtual void setTarget(double q,double qd) {
|
||||
std::scoped_lock lock(mtx_);
|
||||
if (!protocol_) {
|
||||
throw std::runtime_error("Protocol not set for motor");
|
||||
}
|
||||
protocol_->set(node_id_,q, qd);
|
||||
protocol_->setTarget(node_id_,q, qd);
|
||||
}
|
||||
|
||||
virtual bool calibrateZeroQ() {
|
||||
|
||||
@ -27,7 +27,7 @@ namespace cmvr {
|
||||
virtual bool initNode(uint8_t node_id) = 0;
|
||||
|
||||
virtual void setQ(uint8_t node_id, double angle_rad) = 0;
|
||||
virtual void set(uint8_t node_id, double angle_rad,double vel) = 0;
|
||||
virtual void setTarget(uint8_t node_id, double angle_rad,double vel) = 0;
|
||||
virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
|
||||
virtual msgs::RunMode getMode(uint8_t node_id) = 0;
|
||||
virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;
|
||||
|
||||
@ -16,11 +16,11 @@ void Ti5MotorRPDO1::UpdateData(uint8_t *data) {
|
||||
data[2] = target_pos_ >> 16 & 0xFF;
|
||||
data[3] = target_pos_ >> 24 & 0xFF;
|
||||
//
|
||||
data[4] = ctrl_word_ & 0xFF;
|
||||
data[5] = ctrl_word_ >> 8 & 0xFF;
|
||||
// data[4] = ctrl_word_ & 0xFF;
|
||||
// data[5] = ctrl_word_ >> 8 & 0xFF;
|
||||
|
||||
// data[4] = target_vel_ & 0xFF;
|
||||
// data[5] = target_vel_ >> 8 & 0xFF;
|
||||
// data[6] = target_vel_ >> 16 & 0xFF;
|
||||
// data[7] = target_vel_ >> 24 & 0xFF;
|
||||
data[4] = target_vel_ & 0xFF;
|
||||
data[5] = target_vel_ >> 8 & 0xFF;
|
||||
data[6] = target_vel_ >> 16 & 0xFF;
|
||||
data[7] = target_vel_ >> 24 & 0xFF;
|
||||
}
|
||||
|
||||
@ -25,7 +25,7 @@ namespace cmvr {
|
||||
void UpdateData(uint8_t *data) override;
|
||||
|
||||
int32_t GetLength() const override {
|
||||
return 0x06;
|
||||
return 0x08;
|
||||
}
|
||||
|
||||
uint32_t GetPeriod() const override {
|
||||
|
||||
@ -11,10 +11,10 @@
|
||||
using namespace cmvr::device;
|
||||
using namespace cmvr::msgs;
|
||||
|
||||
Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs::RobotDetail> > sender,
|
||||
std::shared_ptr<MessageManager<msgs::RobotDetail> > manager)
|
||||
: can_sender_(sender), message_manager_(manager){
|
||||
|
||||
Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs::RobotDetail>> sender,
|
||||
std::shared_ptr<MessageManager<msgs::RobotDetail>> manager)
|
||||
: can_sender_(sender), message_manager_(manager)
|
||||
{
|
||||
comm_proto = CommProto::CANOPEN;
|
||||
|
||||
// 添加 message
|
||||
@ -26,25 +26,28 @@ Ti5MotorCanopenProtocol::Ti5MotorCanopenProtocol(std::shared_ptr<CanSender<msgs:
|
||||
|
||||
|
||||
// nmt
|
||||
nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail> *>(
|
||||
nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail>*>(
|
||||
message_manager_->GetMutableProtocolDataById(NmtRequestProtocol<RobotDetail>::ID));
|
||||
|
||||
|
||||
if (nmt_command_ == nullptr) {
|
||||
if (nmt_command_ == nullptr)
|
||||
{
|
||||
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
|
||||
}
|
||||
can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true);
|
||||
|
||||
// sync
|
||||
sync_command_ = dynamic_cast<SyncProtocol<RobotDetail> *>(
|
||||
sync_command_ = dynamic_cast<SyncProtocol<RobotDetail>*>(
|
||||
message_manager_->GetMutableProtocolDataById(SyncProtocol<RobotDetail>::ID));
|
||||
if (sync_command_ == nullptr) {
|
||||
if (sync_command_ == nullptr)
|
||||
{
|
||||
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
|
||||
}
|
||||
can_sender_->AddMessage(sync_command_->ID, sync_command_, false);
|
||||
}
|
||||
|
||||
bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
|
||||
bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id)
|
||||
{
|
||||
//nmt
|
||||
message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id);
|
||||
|
||||
@ -53,19 +56,18 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
|
||||
message_manager_->AddRecvProtocolData<motor::Ti5MotorSdoResponse, false>(node_id);
|
||||
|
||||
//TPDO
|
||||
message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO1,false>(node_id);
|
||||
message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO2,false>(node_id);
|
||||
message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO1, false>(node_id);
|
||||
message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO2, false>(node_id);
|
||||
|
||||
//RPDO
|
||||
message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1,false>(node_id);
|
||||
message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1, false>(node_id);
|
||||
|
||||
|
||||
|
||||
|
||||
sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail> *>(
|
||||
sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail>*>(
|
||||
message_manager_->GetMutableProtocolDataById(SdoRequestProtocol<RobotDetail>::ID(node_id)));
|
||||
|
||||
if (sdo_commands_[node_id] == nullptr) {
|
||||
if (sdo_commands_[node_id] == nullptr)
|
||||
{
|
||||
LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
|
||||
return ErrorCode::CANBUS_ERROR;
|
||||
}
|
||||
@ -73,10 +75,11 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
|
||||
|
||||
|
||||
// pdo1
|
||||
rpdo1_commands_[node_id] = dynamic_cast<motor::Ti5MotorRPDO1 *>(
|
||||
rpdo1_commands_[node_id] = dynamic_cast<motor::Ti5MotorRPDO1*>(
|
||||
message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO1::ID(node_id)));
|
||||
|
||||
if (rpdo1_commands_[node_id] == nullptr) {
|
||||
if (rpdo1_commands_[node_id] == nullptr)
|
||||
{
|
||||
LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!";
|
||||
return ErrorCode::CANBUS_ERROR;
|
||||
}
|
||||
@ -87,27 +90,31 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
|
||||
|
||||
|
||||
void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index,
|
||||
uint32_t data, uint32_t delay_ms) {
|
||||
uint32_t data, uint32_t delay_ms)
|
||||
{
|
||||
sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data);
|
||||
can_sender_->Update(sdo_commands_[node_id]->ID());
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) {
|
||||
void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad)
|
||||
{
|
||||
auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
|
||||
|
||||
switch (cur_mode_[node_id]) {
|
||||
case RUN_MODE_CYCLIC_SYNC_POSITION:
|
||||
setCSPTargetPosByPdo(node_id, static_cast<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;
|
||||
switch (cur_mode_[node_id])
|
||||
{
|
||||
case RUN_MODE_CYCLIC_SYNC_POSITION:
|
||||
setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
|
||||
break;
|
||||
case RUN_MODE_PROFILE_POSITION:
|
||||
setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
|
||||
// setPPTargetPosBySdo(node_id,static_cast<int32_t>(cmd));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::set(uint8_t node_id, double angle_rad, double vel) {
|
||||
void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel)
|
||||
{
|
||||
auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
|
||||
auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
|
||||
rpdo1_commands_[node_id]->SetTargetPos(pos_cmd);
|
||||
@ -116,7 +123,8 @@ void Ti5MotorCanopenProtocol::set(uint8_t node_id, double angle_rad, double vel)
|
||||
}
|
||||
|
||||
|
||||
void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
|
||||
void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos)
|
||||
{
|
||||
controlword_t cw = {};
|
||||
cw.switch_on = 1;
|
||||
cw.enable_voltage = 1;
|
||||
@ -138,7 +146,8 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos)
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
|
||||
void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos)
|
||||
{
|
||||
// 触发目标位置运动
|
||||
controlword_t cw;
|
||||
cw.value = 0x0F;
|
||||
@ -156,15 +165,16 @@ void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos)
|
||||
can_sender_->Update(rpdo1_commands_[node_id]->ID());
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
|
||||
void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos)
|
||||
{
|
||||
rpdo1_commands_[node_id]->SetTargetPos(pos);
|
||||
rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
|
||||
can_sender_->Update(rpdo1_commands_[node_id]->ID());
|
||||
}
|
||||
|
||||
|
||||
void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
|
||||
|
||||
void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode)
|
||||
{
|
||||
cur_mode_[node_id] = mode;
|
||||
// 1 : 先设置模式
|
||||
auto data = static_cast<uint32_t>(mode);
|
||||
@ -184,8 +194,10 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
|
||||
|
||||
|
||||
switch (mode) {
|
||||
case RUN_MODE_PROFILE_POSITION: {
|
||||
switch (mode)
|
||||
{
|
||||
case RUN_MODE_PROFILE_POSITION:
|
||||
{
|
||||
// 4 : 设置目标位置（为当前位置）
|
||||
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
|
||||
@ -202,7 +214,8 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
|
||||
break;
|
||||
}
|
||||
|
||||
case RUN_MODE_CYCLIC_SYNC_POSITION: {
|
||||
case RUN_MODE_CYCLIC_SYNC_POSITION:
|
||||
{
|
||||
// 设置目标位置为当前位置
|
||||
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
|
||||
@ -214,33 +227,45 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
|
||||
break;
|
||||
}
|
||||
|
||||
case RUN_MODE_PROFILE_VELOCITY: {
|
||||
case RUN_MODE_PROFILE_VELOCITY:
|
||||
{
|
||||
cw.enable_operation = 1;
|
||||
cw.switch_on = 1;
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
// TODO: Handle unspecified or unknown mode
|
||||
|
||||
case RUN_MODE_CYCLIC_SYNC_VELOCITY:
|
||||
{
|
||||
cw.enable_operation = 1;
|
||||
cw.switch_on = 1;
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
// TODO: Handle unspecified or unknown mode
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
|
||||
void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms)
|
||||
{
|
||||
nmt_command_->RequestService(node_id, command);
|
||||
can_sender_->Update(nmt_command_->ID);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
||||
}
|
||||
|
||||
|
||||
void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
|
||||
void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel)
|
||||
{
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
|
||||
}
|
||||
|
||||
|
||||
void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) {
|
||||
void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id)
|
||||
{
|
||||
//TDPO1 配置 状态字 和 控制字
|
||||
// 1: 失能 pdo
|
||||
uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
|
||||
@ -280,7 +305,8 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) {
|
||||
}
|
||||
|
||||
|
||||
void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) {
|
||||
void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id)
|
||||
{
|
||||
// 1: 失能 pdo
|
||||
uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
|
||||
@ -310,7 +336,8 @@ void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) {
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) {
|
||||
void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start)
|
||||
{
|
||||
// 1: 失能 pdo
|
||||
uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
|
||||
@ -331,10 +358,11 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) {
|
||||
|
||||
//6 : 映射控制字
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
|
||||
CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
|
||||
TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32);
|
||||
|
||||
|
||||
if (start) {
|
||||
if (start)
|
||||
{
|
||||
//7 写入该PDO映射对象总个数
|
||||
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2);
|
||||
|
||||
@ -344,47 +372,53 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) {
|
||||
}
|
||||
|
||||
|
||||
void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) {
|
||||
void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id)
|
||||
{
|
||||
configTPDO1(node_id);
|
||||
configTPDO2(node_id);
|
||||
configRPDO1(node_id, true);
|
||||
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) {
|
||||
void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd)
|
||||
{
|
||||
auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
|
||||
auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id,double qd) {
|
||||
void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd)
|
||||
{
|
||||
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed);
|
||||
// seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed);
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) {
|
||||
ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0;
|
||||
lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0;
|
||||
void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb)
|
||||
{
|
||||
ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0;
|
||||
lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0;
|
||||
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
|
||||
}
|
||||
|
||||
|
||||
bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
|
||||
|
||||
bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id)
|
||||
{
|
||||
// 0: 设置控制字为 0x06，确保停机状态
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06,1000);
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000);
|
||||
|
||||
// 1: 清除偏置值 0x2008 ← 0
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
|
||||
|
||||
// 2: 等待确认清除成功
|
||||
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
|
||||
if (!waitUntil([&]() {
|
||||
if (!waitUntil([&]()
|
||||
{
|
||||
return GetRobotDetail()->motors().at(node_id).position_offset() == 0;
|
||||
}, 1000)) {
|
||||
}, 1000))
|
||||
{
|
||||
LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed";
|
||||
return false;
|
||||
}
|
||||
@ -397,14 +431,16 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
|
||||
|
||||
// 5: 保存参数到永久区（0x2000 ← 1）
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1,100);
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100);
|
||||
|
||||
|
||||
// 6: 确认写入成功
|
||||
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
|
||||
if (!waitUntil([&]() {
|
||||
if (!waitUntil([&]()
|
||||
{
|
||||
return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos;
|
||||
}, 500)) {
|
||||
}, 500))
|
||||
{
|
||||
return false;
|
||||
LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed";
|
||||
}
|
||||
@ -412,61 +448,72 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
|
||||
return true;
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::brake(uint8_t node_id) {
|
||||
|
||||
void Ti5MotorCanopenProtocol::brake(uint8_t node_id)
|
||||
{
|
||||
// // 开机未使能电机时调用
|
||||
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
|
||||
// 6 抱闸 0 ： 立即停机 自由
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0);
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02,100);
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100);
|
||||
|
||||
// 必须要发送 0xf 才能按照6085中设定的减速度减速
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
|
||||
}
|
||||
|
||||
bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) {
|
||||
bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id)
|
||||
{
|
||||
statusword_t st{};
|
||||
st.value = GetRobotDetail()->motors().at(node_id).status_word();
|
||||
return st.target_reached == 1 ;
|
||||
return st.target_reached == 1;
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) {
|
||||
void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd)
|
||||
{
|
||||
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
|
||||
switch (cur_mode_[node_id]) {
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
|
||||
case msgs::RUN_MODE_PROFILE_POSITION: {
|
||||
switch (cur_mode_[node_id])
|
||||
{
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
|
||||
case msgs::RUN_MODE_PROFILE_POSITION:
|
||||
{
|
||||
auto it = last_Qd_.find(node_id);
|
||||
if (it == last_Qd_.end() || it->second != speed) {
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),0);
|
||||
if (it == last_Qd_.end() || it->second != speed)
|
||||
{
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),
|
||||
0);
|
||||
last_Qd_[node_id] = speed;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case msgs::RUN_MODE_PROFILE_VELOCITY: {
|
||||
// 在速度模式下，直接设置目标速度
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
case msgs::RUN_MODE_PROFILE_VELOCITY:
|
||||
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
|
||||
{
|
||||
// 在速度模式下，直接设置目标速度
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd) {
|
||||
void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd)
|
||||
{
|
||||
uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0);
|
||||
auto it = last_Qdd_.find(node_id);
|
||||
if (it == last_Qdd_.end() || it->second != accel) {
|
||||
if (it == last_Qdd_.end() || it->second != accel)
|
||||
{
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
|
||||
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel);
|
||||
last_Qdd_[node_id] = accel;
|
||||
}
|
||||
}
|
||||
|
||||
void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
|
||||
void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id)
|
||||
{
|
||||
// 0 ： 立即停机 自由
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02,20);
|
||||
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20);
|
||||
// 必须要发送 0xf 才能按照6085中设定的减速度减速
|
||||
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
|
||||
|
||||
@ -475,19 +522,18 @@ void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
|
||||
}
|
||||
|
||||
|
||||
double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) {
|
||||
double Ti5MotorCanopenProtocol::getQ(uint8_t node_id)
|
||||
{
|
||||
auto data_ptr = std::make_unique<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);
|
||||
auto cnt = data_ptr->motors().at(node_id).position();
|
||||
return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG);
|
||||
}
|
||||
|
||||
double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) {
|
||||
double Ti5MotorCanopenProtocol::getQd(uint8_t node_id)
|
||||
{
|
||||
auto data_ptr = std::make_unique<msgs::RobotDetail>();
|
||||
message_manager_->GetSensorData(data_ptr.get());
|
||||
auto cnt = data_ptr->motors().at(node_id).speed();
|
||||
return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
|
||||
auto cnt = data_ptr->motors().at(node_id).speed();
|
||||
return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -29,7 +29,7 @@ namespace cmvr {
|
||||
bool initNode(uint8_t node_id) override;
|
||||
|
||||
void setMode(uint8_t node_id, msgs::RunMode mode);
|
||||
void set(uint8_t node_id, double angle_rad, double vel) override;
|
||||
void setTarget(uint8_t node_id, double angle_rad, double vel) override;
|
||||
|
||||
void setQ(uint8_t node_id, double angle_rad) override;
|
||||
void setLimitQ(uint8_t node_id, double ub, double lb) override;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user