83 lines
2.6 KiB
C++
83 lines
2.6 KiB
C++
//
|
|
// Created by linbo on 2025/6/20.
|
|
//
|
|
|
|
#ifndef RH56DFTP_DEXHAND_H
|
|
#define RH56DFTP_DEXHAND_H
|
|
|
|
#include <thread>
|
|
#include <mutex>
|
|
#include <queue>
|
|
#include <condition_variable>
|
|
#include <modbus/modbus.h>
|
|
#include <boost/lockfree/spsc_queue.hpp>
|
|
#include "../../../../utils/base/include/os.h"
|
|
#include "../../abstract_dexhand.h"
|
|
#include "../../../../hardware/include/serial_interface.h"
|
|
|
|
namespace cmvr::device {
|
|
|
|
class ModbusController {
|
|
private:
|
|
modbus_t *ctx;
|
|
std::map<std::string, int> regdict;
|
|
static constexpr int MAX_REGISTERS_PER_READ = 64; // 根据Modbus协议限制设置
|
|
|
|
public:
|
|
ModbusController();
|
|
~ModbusController();
|
|
|
|
bool open(const std::string& ip, int port);
|
|
void close();
|
|
|
|
bool writeRegisters(int address, const std::vector<uint16_t>& values);
|
|
std::vector<uint16_t> readRegisters(int address, int count);
|
|
std::vector<uint16_t> readRegisterRange(int start_addr, int end_addr);
|
|
|
|
bool write6(const std::string& reg_name, const std::vector<int>& val);
|
|
bool read6(const std::string& reg_name, std::vector<int>& result);
|
|
|
|
static void sleep(float seconds);
|
|
};
|
|
|
|
class RH56DFTPDexhand final : public AbstractDexHand{
|
|
public:
|
|
explicit RH56DFTPDexhand(const XmlNode& cfg);
|
|
RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg);
|
|
~RH56DFTPDexhand() override;
|
|
|
|
void init() override;
|
|
void start() override;
|
|
void stop() override;
|
|
|
|
|
|
void getState(DexHandState &state) override;
|
|
|
|
// control
|
|
void setPositions(const std::vector<int>& finger_joint_targets) override;
|
|
void setAngles(const std::vector<int>& finger_joint_angles) override;
|
|
void setVelocities(const std::vector<int>& finger_joint_velocities) override;
|
|
void setPresetAct(int action_id) override;//这个接口暂时不可用，未开放寄存器设置预设
|
|
void execPresetAct(int action_id) override;//这个无用
|
|
void setForce(const std::vector<int>& finger_joint_force) override;
|
|
HandTactileSensors& getSensorData() override;
|
|
HandTactileSensors& getSensorData(FingerType finger_type, TactileRegion tactile_region) override;
|
|
private:
|
|
void updateState();
|
|
void updateSensorData();
|
|
|
|
int default_force_;//上电的力控阈值
|
|
int default_speed_;//上电的自由角转动速度
|
|
|
|
std::string ip_address_;
|
|
int port_;
|
|
std::shared_ptr<ModbusController> controller_;
|
|
|
|
config::RH56DFTPDexHandConfig dexhandCfg_;
|
|
};
|
|
}
|
|
|
|
|
|
|
|
#endif //RH56DFTP_DEXHAND_H
|