110 lines
4.3 KiB
C++
110 lines
4.3 KiB
C++
//
|
|
// Created by linbo on 2025/10/22.
|
|
//
|
|
|
|
#pragma once
|
|
|
|
#include "rapidxml/xml_parser.h"
|
|
#include "abstractmotorprotocol.h"
|
|
#include "cmvr/msgs/motor.pb.h"
|
|
#include "motor/motor_protocol_interface.h"
|
|
#include "devices/abstract_canbus.h"
|
|
#include "canbus/can_comm/can_receiver.h"
|
|
#include "canbus/can_comm/can_sender.h"
|
|
#include "canbus/can_comm/message_manager.h"
|
|
#include "cmvr/msgs/error_code.pb.h"
|
|
#include "cmvr/msgs/robot_detail.pb.h"
|
|
|
|
#include "canbus/canopen/sdo_request_protocol.h"
|
|
#include "canbus/canopen/sync_protocol.h"
|
|
#include "canbus/canopen/nmt_request_protocol.h"
|
|
#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h"
|
|
#include <cmath>
|
|
namespace cmvr::hardware{
|
|
class Ti5MotorProtocol final : public AbstractMotorProtocol {
|
|
public:
|
|
explicit Ti5MotorProtocol(const XmlNode &cfg);
|
|
Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
|
|
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
|
|
~Ti5MotorProtocol() override;
|
|
|
|
bool initNode(uint8_t node_id) override;
|
|
|
|
void setMode(uint8_t node_id, msgs::RunMode mode) override;
|
|
|
|
void setQ(uint8_t node_id, double angle_rad) 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 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;
|
|
|
|
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 configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
|
|
void configPdo(uint8_t node_id);
|
|
|
|
std::unique_ptr<msgs::RobotDetail> GetRobotDetail() {
|
|
auto data_ptr = std::make_unique<msgs::RobotDetail>();
|
|
message_manager_->GetSensorData(data_ptr.get());
|
|
return data_ptr;
|
|
}
|
|
|
|
msgs::RunMode getMode(uint8_t node_id) override {
|
|
return cur_mode_[node_id];
|
|
}
|
|
|
|
|
|
private:
|
|
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 configTPDO1(uint8_t node_id);
|
|
void configTPDO2(uint8_t node_id);
|
|
// 目标位置 607A + 控制字 6040
|
|
void configRPDO1(uint8_t node_id, bool start);
|
|
bool waitUntil(std::function<bool()> condition, int timeout_ms) {
|
|
auto start = std::chrono::steady_clock::now();
|
|
while (!condition()) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
if (std::chrono::steady_clock::now() - start > std::chrono::milliseconds(timeout_ms))
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
private:
|
|
static constexpr double GearRatio = 101.0; // 电机减速比
|
|
static constexpr double RADTODEG = 180.0 / M_PI;
|
|
std::shared_ptr<device::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::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
|
|
std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
|
|
|
|
// nmt
|
|
device::NmtRequestProtocol<msgs::RobotDetail> *nmt_command_{nullptr};
|
|
|
|
//sync
|
|
device::SyncProtocol<msgs::RobotDetail> *sync_command_{nullptr};
|
|
|
|
// sdo
|
|
std::map<uint8_t, device::SdoRequestProtocol<msgs::RobotDetail> *> sdo_commands_{};
|
|
|
|
// rpdo1
|
|
std::map<uint8_t, device::motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
|
|
};
|
|
} |