fix: improve RH56 tactile data read speed
This commit is contained in:
parent
9faa2a3852
commit
d02d23a3f9
@ -57,14 +57,6 @@ public:
|
||||
ROBOT_COMMAND_FAILED // 向机器人下发控制命令失败。
|
||||
};
|
||||
|
||||
enum class TactileRegion {
|
||||
TIP = 0,
|
||||
FINGER,
|
||||
PAD,
|
||||
TIP_AND_FINGER,
|
||||
THUMB_MIDDLE
|
||||
};
|
||||
|
||||
enum class AlignMode {
|
||||
POSE_AND_POSITION = 0, // 使用配置里的固定 rx/ry/rz 与位置一起对齐。
|
||||
RX_RY_AND_POSITION, // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。
|
||||
@ -202,11 +194,14 @@ public:
|
||||
// 使用哪根手指的触觉阵列判断是否接触。
|
||||
device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX};
|
||||
// 使用该手指的哪个触觉区域。
|
||||
TactileRegion tactile_region{TactileRegion::TIP_AND_FINGER};
|
||||
device::AbstractDexHand::TactileRegion tactile_region{
|
||||
device::AbstractDexHand::TactileRegion::TIP};
|
||||
// 触觉压力和阈值；总和超过该值认为已经接触。
|
||||
double tactile_pressure_sum_threshold{100.0};
|
||||
// 触觉峰值阈值；为 0 时表示不使用峰值判据。
|
||||
double tactile_pressure_peak_threshold{0.0};
|
||||
// 当前帧非 0 触觉点数量阈值；只有达到该数量后，才进一步判断 sum/peak 阈值。
|
||||
int tactile_nonzero_count_threshold{1};
|
||||
};
|
||||
|
||||
TouchScreenApp();
|
||||
@ -244,6 +239,7 @@ public:
|
||||
int targetV() const { return target_v_; }
|
||||
double lastTouchPressureSum() const { return last_touch_pressure_sum_; }
|
||||
double lastTouchPressurePeak() const { return last_touch_pressure_peak_; }
|
||||
int lastTouchNonzeroCount() const { return last_touch_nonzero_count_; }
|
||||
int lastActiveTagId() const { return last_active_tag_id_; }
|
||||
const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; }
|
||||
|
||||
@ -306,6 +302,7 @@ private:
|
||||
|
||||
double last_touch_pressure_sum_{0.0};
|
||||
double last_touch_pressure_peak_{0.0};
|
||||
int last_touch_nonzero_count_{0};
|
||||
Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
|
||||
bool locked_target_rotation_valid_{false};
|
||||
Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
|
||||
|
||||
@ -45,14 +45,20 @@ bool computeLinearMoveDeltaTool(const Eigen::Matrix<double, 6, 1>& twist_base,
|
||||
}
|
||||
|
||||
bool isTouchTriggered(const TouchScreenApp::Options& options,
|
||||
const int nonzero_count,
|
||||
const double pressure_sum,
|
||||
const double pressure_peak) {
|
||||
if (options.tactile_nonzero_count_threshold > 0 &&
|
||||
nonzero_count < options.tactile_nonzero_count_threshold) {
|
||||
return false;
|
||||
}
|
||||
return pressure_sum >= options.tactile_pressure_sum_threshold ||
|
||||
(options.tactile_pressure_peak_threshold > 0.0 &&
|
||||
pressure_peak >= options.tactile_pressure_peak_threshold);
|
||||
}
|
||||
|
||||
void accumulateMatrixStats(const device::AbstractDexHand::TactileMatrixView& matrix,
|
||||
int& nonzero_count_out,
|
||||
double& sum_out,
|
||||
double& peak_out) {
|
||||
if (!matrix.valid()) {
|
||||
@ -63,6 +69,9 @@ void accumulateMatrixStats(const device::AbstractDexHand::TactileMatrixView& mat
|
||||
const auto* row_data = matrix.rowData(row);
|
||||
for (int col = 0; col < matrix.cols; ++col) {
|
||||
const double value = static_cast<double>(row_data[col]);
|
||||
if (row_data[col] != 0) {
|
||||
++nonzero_count_out;
|
||||
}
|
||||
sum_out += value;
|
||||
peak_out = std::max(peak_out, value);
|
||||
}
|
||||
@ -127,27 +136,48 @@ Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) {
|
||||
return aa.axis() * aa.angle();
|
||||
}
|
||||
|
||||
bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target,
|
||||
const Eigen::Matrix3d& R_current,
|
||||
double& yaw_rad_out) {
|
||||
if (!R_target.allFinite() || !R_current.allFinite()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const Eigen::Vector3d z_ref = R_target.col(2);
|
||||
const Eigen::Vector3d x_ref = R_target.col(0);
|
||||
const Eigen::Vector3d y_ref = R_target.col(1);
|
||||
|
||||
Eigen::Vector3d in_plane = R_current.col(0) - z_ref * z_ref.dot(R_current.col(0));
|
||||
if (in_plane.norm() <= 1e-9) {
|
||||
in_plane = R_current.col(1) - z_ref * z_ref.dot(R_current.col(1));
|
||||
}
|
||||
const double in_plane_norm = in_plane.norm();
|
||||
if (!std::isfinite(in_plane_norm) || in_plane_norm <= 1e-9) {
|
||||
return false;
|
||||
}
|
||||
|
||||
in_plane /= in_plane_norm;
|
||||
yaw_rad_out = std::atan2(y_ref.dot(in_plane), x_ref.dot(in_plane));
|
||||
return std::isfinite(yaw_rad_out);
|
||||
}
|
||||
|
||||
bool appendRequestedTactileRegions(
|
||||
const device::AbstractDexHand::FingerType finger,
|
||||
const TouchScreenApp::TactileRegion region,
|
||||
const device::AbstractDexHand::TactileRegion region,
|
||||
std::vector<device::AbstractDexHand::TactileRegionKey>& regions_out) {
|
||||
using DeviceTactileRegion = device::AbstractDexHand::TactileRegion;
|
||||
|
||||
switch (region) {
|
||||
case TouchScreenApp::TactileRegion::TIP:
|
||||
case device::AbstractDexHand::TactileRegion::TIP:
|
||||
regions_out.emplace_back(finger, DeviceTactileRegion::TIP);
|
||||
return true;
|
||||
case TouchScreenApp::TactileRegion::FINGER:
|
||||
case device::AbstractDexHand::TactileRegion::FINGER:
|
||||
regions_out.emplace_back(finger, DeviceTactileRegion::FINGER);
|
||||
return true;
|
||||
case TouchScreenApp::TactileRegion::PAD:
|
||||
case device::AbstractDexHand::TactileRegion::PAD:
|
||||
regions_out.emplace_back(finger, DeviceTactileRegion::PAD);
|
||||
return true;
|
||||
case TouchScreenApp::TactileRegion::TIP_AND_FINGER:
|
||||
regions_out.emplace_back(finger, DeviceTactileRegion::TIP);
|
||||
regions_out.emplace_back(finger, DeviceTactileRegion::FINGER);
|
||||
return true;
|
||||
case TouchScreenApp::TactileRegion::THUMB_MIDDLE:
|
||||
case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE:
|
||||
if (finger != device::AbstractDexHand::FingerType::THUMB) {
|
||||
return false;
|
||||
}
|
||||
@ -197,21 +227,42 @@ device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreen
|
||||
return device::AbstractDexHand::FingerType::INDEX;
|
||||
}
|
||||
|
||||
TouchScreenApp::TactileRegion toTactileRegion(
|
||||
const char* fingerTypeToString(const device::AbstractDexHand::FingerType finger) {
|
||||
switch (finger) {
|
||||
case device::AbstractDexHand::FingerType::PINKY: return "PINKY";
|
||||
case device::AbstractDexHand::FingerType::RING: return "RING";
|
||||
case device::AbstractDexHand::FingerType::MIDDLE: return "MIDDLE";
|
||||
case device::AbstractDexHand::FingerType::INDEX: return "INDEX";
|
||||
case device::AbstractDexHand::FingerType::THUMB: return "THUMB";
|
||||
case device::AbstractDexHand::FingerType::PALM: return "PALM";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
|
||||
const char* tactileRegionToString(const device::AbstractDexHand::TactileRegion region) {
|
||||
switch (region) {
|
||||
case device::AbstractDexHand::TactileRegion::TIP: return "TIP";
|
||||
case device::AbstractDexHand::TactileRegion::FINGER: return "FINGER";
|
||||
case device::AbstractDexHand::TactileRegion::PAD: return "PAD";
|
||||
case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE";
|
||||
case device::AbstractDexHand::TactileRegion::PALM_PAD: return "PALM_PAD";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
|
||||
device::AbstractDexHand::TactileRegion toTactileRegion(
|
||||
const cmvr::config::TouchScreenTactileRegion region) {
|
||||
switch (region) {
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP:
|
||||
return TouchScreenApp::TactileRegion::TIP;
|
||||
return device::AbstractDexHand::TactileRegion::TIP;
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER:
|
||||
return TouchScreenApp::TactileRegion::FINGER;
|
||||
return device::AbstractDexHand::TactileRegion::FINGER;
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD:
|
||||
return TouchScreenApp::TactileRegion::PAD;
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER:
|
||||
return TouchScreenApp::TactileRegion::TIP_AND_FINGER;
|
||||
return device::AbstractDexHand::TactileRegion::PAD;
|
||||
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE:
|
||||
return TouchScreenApp::TactileRegion::THUMB_MIDDLE;
|
||||
return device::AbstractDexHand::TactileRegion::THUMB_MIDDLE;
|
||||
}
|
||||
return TouchScreenApp::TactileRegion::TIP;
|
||||
return device::AbstractDexHand::TactileRegion::TIP;
|
||||
}
|
||||
|
||||
TouchScreenApp::AlignMode toAlignMode(const cmvr::config::TouchScreenAlignMode mode) {
|
||||
@ -445,6 +496,7 @@ bool TouchScreenApp::init(const std::shared_ptr<device::AbstractRobot>& robot,
|
||||
last_active_tag_id_ = -1;
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_pressure_peak_ = 0.0;
|
||||
last_touch_nonzero_count_ = 0;
|
||||
last_align_error_camera_.setZero();
|
||||
last_status_ = Status::IDLE;
|
||||
} else {
|
||||
@ -539,6 +591,7 @@ bool TouchScreenApp::startFromPixel(int u, int v) {
|
||||
align_stable_count_ = 0;
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_pressure_peak_ = 0.0;
|
||||
last_touch_nonzero_count_ = 0;
|
||||
last_active_tag_id_ = -1;
|
||||
last_align_error_camera_.setZero();
|
||||
locked_target_rotation_valid_ = false;
|
||||
@ -560,7 +613,15 @@ bool TouchScreenApp::step() {
|
||||
}
|
||||
|
||||
if (dexhand_) {
|
||||
updateTouchPressure();
|
||||
const bool tactile_ok = updateTouchPressure();
|
||||
// std::cout << "[TouchScreenApp][TACTILE] finger="
|
||||
// << fingerTypeToString(options_.tactile_finger)
|
||||
// << ", region=" << tactileRegionToString(options_.tactile_region)
|
||||
// << ", ok=" << (tactile_ok ? 1 : 0)
|
||||
// << ", nonzero_count=" << last_touch_nonzero_count_
|
||||
// << ", pressure_sum=" << last_touch_pressure_sum_
|
||||
// << ", pressure_peak=" << last_touch_pressure_peak_
|
||||
// << std::endl;
|
||||
}
|
||||
|
||||
switch (phase_) {
|
||||
@ -621,6 +682,7 @@ void TouchScreenApp::stop() {
|
||||
last_active_tag_id_ = -1;
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_pressure_peak_ = 0.0;
|
||||
last_touch_nonzero_count_ = 0;
|
||||
last_align_error_camera_.setZero();
|
||||
locked_target_rotation_valid_ = false;
|
||||
locked_target_rotation_.setIdentity();
|
||||
@ -874,6 +936,10 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
|
||||
if (config.has_tactile_pressure_peak_threshold()) {
|
||||
options.tactile_pressure_peak_threshold = config.tactile_pressure_peak_threshold();
|
||||
}
|
||||
if (config.has_tactile_nonzero_count_threshold()) {
|
||||
options.tactile_nonzero_count_threshold =
|
||||
std::max(0, config.tactile_nonzero_count_threshold());
|
||||
}
|
||||
|
||||
options_out = options;
|
||||
return true;
|
||||
@ -1042,9 +1108,8 @@ bool TouchScreenApp::stepAligning() {
|
||||
break;
|
||||
case AlignMode::RX_RY_AND_POSITION:
|
||||
if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) {
|
||||
const Eigen::Matrix3d R_delta = R_target.transpose() * R_current;
|
||||
const double locked_yaw_rad = std::atan2(R_delta(1, 0), R_delta(0, 0));
|
||||
if (!std::isfinite(locked_yaw_rad)) {
|
||||
double locked_yaw_rad = 0.0;
|
||||
if (!extractProjectedYawAboutTargetNormal(R_target, R_current, locked_yaw_rad)) {
|
||||
enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
|
||||
return false;
|
||||
}
|
||||
@ -1170,7 +1235,10 @@ bool TouchScreenApp::stepTouching() {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) {
|
||||
if (isTouchTriggered(options_,
|
||||
last_touch_nonzero_count_,
|
||||
last_touch_pressure_sum_,
|
||||
last_touch_pressure_peak_)) {
|
||||
if (options_.touch_use_speedl) {
|
||||
logTouchingSpeedLState();
|
||||
}
|
||||
@ -1206,7 +1274,10 @@ bool TouchScreenApp::stepTouching() {
|
||||
enterFailed(Status::TACTILE_UNAVAILABLE);
|
||||
return false;
|
||||
}
|
||||
if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) {
|
||||
if (isTouchTriggered(options_,
|
||||
last_touch_nonzero_count_,
|
||||
last_touch_pressure_sum_,
|
||||
last_touch_pressure_peak_)) {
|
||||
if (!handleTouchTriggered(true)) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
@ -1562,6 +1633,7 @@ void TouchScreenApp::enterFailed(const Status status) {
|
||||
bool TouchScreenApp::updateTouchPressure() {
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_pressure_peak_ = 0.0;
|
||||
last_touch_nonzero_count_ = 0;
|
||||
if (!dexhand_) {
|
||||
return false;
|
||||
}
|
||||
@ -1573,6 +1645,7 @@ bool TouchScreenApp::updateTouchPressure() {
|
||||
return false;
|
||||
}
|
||||
|
||||
int nonzero_count = 0;
|
||||
double sum = 0.0;
|
||||
double peak = 0.0;
|
||||
try {
|
||||
@ -1581,12 +1654,13 @@ bool TouchScreenApp::updateTouchPressure() {
|
||||
if (!sensor.valid()) {
|
||||
return false;
|
||||
}
|
||||
accumulateMatrixStats(sensor.view, sum, peak);
|
||||
accumulateMatrixStats(sensor.view, nonzero_count, sum, peak);
|
||||
}
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
|
||||
last_touch_nonzero_count_ = nonzero_count;
|
||||
last_touch_pressure_sum_ = sum;
|
||||
last_touch_pressure_peak_ = peak;
|
||||
return true;
|
||||
|
||||
@ -45,6 +45,7 @@ void run_touch_once(int u, int v) {
|
||||
<< ", target_c=[" << p_c_target.x() << ", "
|
||||
<< p_c_target.y() << ", "
|
||||
<< p_c_target.z() << "]"
|
||||
<< ", nonzero_count=" << app.lastTouchNonzeroCount()
|
||||
<< ", pressure_sum=" << app.lastTouchPressureSum()
|
||||
<< ", pressure_peak=" << app.lastTouchPressurePeak()
|
||||
<< ", err_c=[" << app.lastAlignErrorCamera().x() << ", "
|
||||
@ -58,7 +59,8 @@ void run_touch_once(int u, int v) {
|
||||
align_reached = true;
|
||||
}
|
||||
if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) {
|
||||
std::cout << "touch triggered, pressure_sum=" << app.lastTouchPressureSum()
|
||||
std::cout << "touch triggered, nonzero_count=" << app.lastTouchNonzeroCount()
|
||||
<< ", pressure_sum=" << app.lastTouchPressureSum()
|
||||
<< ", pressure_peak=" << app.lastTouchPressurePeak() << "\n";
|
||||
touch_triggered = true;
|
||||
}
|
||||
|
||||
@ -135,7 +135,8 @@ retract_twist_base {
|
||||
retract_acceleration: 3.0
|
||||
retract_duration_s: 0.45
|
||||
|
||||
tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
|
||||
tactile_finger: TOUCH_SCREEN_FINGER_TYPE_RING
|
||||
tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP
|
||||
tactile_pressure_sum_threshold: 300000.0
|
||||
tactile_pressure_peak_threshold: 0.0
|
||||
tactile_pressure_sum_threshold: 200.0
|
||||
tactile_pressure_peak_threshold: 0
|
||||
tactile_nonzero_count_threshold: 4
|
||||
|
||||
@ -35,7 +35,7 @@ namespace cmvr::device {
|
||||
bool isOpen() const;
|
||||
|
||||
void writeRegisters(int address, const uint16_t* values, int count);
|
||||
void readRegisterBlock(int start_addr, int count, std::vector<uint16_t>& values);
|
||||
void readRegisterBlock(int start_address, int count, std::vector<uint16_t>& values);
|
||||
|
||||
private:
|
||||
void closeUnlocked();
|
||||
@ -121,7 +121,7 @@ namespace cmvr::device {
|
||||
std::thread tactile_thread_;
|
||||
std::atomic<bool> tactile_thread_running_{false};
|
||||
std::atomic<int> active_buffer_index_{0};
|
||||
std::chrono::milliseconds tactile_poll_interval_{20};
|
||||
std::chrono::milliseconds tactile_poll_interval_{10};
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -12,6 +12,7 @@ namespace cmvr::device {
|
||||
|
||||
enum class RH56TactileFillOrder {
|
||||
ROW_MAJOR,
|
||||
COLUMN_MAJOR_TOP_TO_BOTTOM,
|
||||
COLUMN_MAJOR_BOTTOM_TO_TOP
|
||||
};
|
||||
|
||||
@ -57,8 +58,10 @@ namespace cmvr::device {
|
||||
return;
|
||||
}
|
||||
|
||||
const bool reverse_rows = layout.fill_order == RH56TactileFillOrder::COLUMN_MAJOR_BOTTOM_TO_TOP;
|
||||
for (int col = 0; col < layout.cols; ++col) {
|
||||
for (int row = layout.rows - 1; row >= 0 && input_index < valueCount; --row, ++input_index) {
|
||||
for (int row_index = 0; row_index < layout.rows && input_index < valueCount; ++row_index, ++input_index) {
|
||||
const int row = reverse_rows ? (layout.rows - 1 - row_index) : row_index;
|
||||
destination[static_cast<size_t>(row * layout.cols + col)] = sanitize_point(input[input_index]);
|
||||
}
|
||||
}
|
||||
@ -114,7 +117,7 @@ namespace cmvr::device {
|
||||
14,
|
||||
950,
|
||||
112,
|
||||
RH56TactileFillOrder::COLUMN_MAJOR_BOTTOM_TO_TOP}
|
||||
RH56TactileFillOrder::COLUMN_MAJOR_TOP_TO_BOTTOM}
|
||||
}};
|
||||
return layouts;
|
||||
}
|
||||
|
||||
@ -23,8 +23,10 @@ namespace {
|
||||
constexpr int kDefaultPort = 6000;
|
||||
constexpr int kMaxRegistersPerRead = 125;
|
||||
|
||||
// The RH56 protocol documents addresses and lengths in bytes, while libmodbus
|
||||
// expects holding-register indexes (16-bit words).
|
||||
// The RH56 tactile map uses byte-style protocol addresses (3000, 3370, ...)
|
||||
// while each Modbus read still returns 16-bit registers. Keep addresses in
|
||||
// the protocol units that the datasheet/Python script use, and only convert
|
||||
// byte lengths or address deltas to register counts when slicing buffers.
|
||||
constexpr int kRegisterByteWidth = 2;
|
||||
constexpr int kFingerTipByteLength = 18; // 3 x 3
|
||||
constexpr int kFingerSurfaceByteLength = 192; // 12 x 8
|
||||
@ -37,10 +39,6 @@ namespace {
|
||||
return byte_length / kRegisterByteWidth;
|
||||
}
|
||||
|
||||
constexpr int toRegisterAddress(const int byte_address) {
|
||||
return byte_address / kRegisterByteWidth;
|
||||
}
|
||||
|
||||
constexpr int nextByteAddress(const int start_byte_addr, const int byte_length) {
|
||||
return start_byte_addr + byte_length;
|
||||
}
|
||||
@ -57,12 +55,12 @@ namespace {
|
||||
struct TactileRegionConfig {
|
||||
FingerType finger;
|
||||
TactileRegion region;
|
||||
int start_register;
|
||||
int start_address;
|
||||
int register_count;
|
||||
};
|
||||
|
||||
struct TactileReadSegment {
|
||||
int start_register;
|
||||
int start_address;
|
||||
int register_count;
|
||||
std::vector<size_t> region_indexes;
|
||||
};
|
||||
@ -72,84 +70,84 @@ namespace {
|
||||
const std::array<TactileRegionConfig, RH56DFTPDexhand::TACTILE_REGION_SLOT_COUNT> kTactileRegionConfigs = {{
|
||||
{FingerType::PINKY,
|
||||
TactileRegion::TIP,
|
||||
toRegisterAddress(kTouchSensorBaseByteAddrPinky),
|
||||
kTouchSensorBaseByteAddrPinky,
|
||||
toRegisterCount(kFingerTipByteLength)},
|
||||
{FingerType::PINKY,
|
||||
TactileRegion::FINGER,
|
||||
toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength)),
|
||||
nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength),
|
||||
toRegisterCount(kFingerSurfaceByteLength)},
|
||||
{FingerType::PINKY,
|
||||
TactileRegion::PAD,
|
||||
toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength)),
|
||||
nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrPinky, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength),
|
||||
toRegisterCount(kFingerPadByteLength)},
|
||||
|
||||
{FingerType::RING,
|
||||
TactileRegion::TIP,
|
||||
toRegisterAddress(kTouchSensorBaseByteAddrRing),
|
||||
kTouchSensorBaseByteAddrRing,
|
||||
toRegisterCount(kFingerTipByteLength)},
|
||||
{FingerType::RING,
|
||||
TactileRegion::FINGER,
|
||||
toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength)),
|
||||
nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength),
|
||||
toRegisterCount(kFingerSurfaceByteLength)},
|
||||
{FingerType::RING,
|
||||
TactileRegion::PAD,
|
||||
toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength)),
|
||||
nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrRing, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength),
|
||||
toRegisterCount(kFingerPadByteLength)},
|
||||
|
||||
{FingerType::MIDDLE,
|
||||
TactileRegion::TIP,
|
||||
toRegisterAddress(kTouchSensorBaseByteAddrMiddle),
|
||||
kTouchSensorBaseByteAddrMiddle,
|
||||
toRegisterCount(kFingerTipByteLength)},
|
||||
{FingerType::MIDDLE,
|
||||
TactileRegion::FINGER,
|
||||
toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength)),
|
||||
nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength),
|
||||
toRegisterCount(kFingerSurfaceByteLength)},
|
||||
{FingerType::MIDDLE,
|
||||
TactileRegion::PAD,
|
||||
toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength)),
|
||||
nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrMiddle, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength),
|
||||
toRegisterCount(kFingerPadByteLength)},
|
||||
|
||||
{FingerType::INDEX,
|
||||
TactileRegion::TIP,
|
||||
toRegisterAddress(kTouchSensorBaseByteAddrIndex),
|
||||
kTouchSensorBaseByteAddrIndex,
|
||||
toRegisterCount(kFingerTipByteLength)},
|
||||
{FingerType::INDEX,
|
||||
TactileRegion::FINGER,
|
||||
toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength)),
|
||||
nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength),
|
||||
toRegisterCount(kFingerSurfaceByteLength)},
|
||||
{FingerType::INDEX,
|
||||
TactileRegion::PAD,
|
||||
toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength)),
|
||||
nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrIndex, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength),
|
||||
toRegisterCount(kFingerPadByteLength)},
|
||||
|
||||
{FingerType::THUMB,
|
||||
TactileRegion::TIP,
|
||||
toRegisterAddress(kTouchSensorBaseByteAddrThumb),
|
||||
kTouchSensorBaseByteAddrThumb,
|
||||
toRegisterCount(kFingerTipByteLength)},
|
||||
{FingerType::THUMB,
|
||||
TactileRegion::FINGER,
|
||||
toRegisterAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength)),
|
||||
nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength),
|
||||
toRegisterCount(kFingerSurfaceByteLength)},
|
||||
{FingerType::THUMB,
|
||||
TactileRegion::THUMB_MIDDLE,
|
||||
toRegisterAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength)),
|
||||
nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb, kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength),
|
||||
toRegisterCount(kThumbMiddleByteLength)},
|
||||
{FingerType::THUMB,
|
||||
TactileRegion::PAD,
|
||||
toRegisterAddress(nextByteAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb,
|
||||
nextByteAddress(nextByteAddress(nextByteAddress(kTouchSensorBaseByteAddrThumb,
|
||||
kFingerTipByteLength),
|
||||
kFingerSurfaceByteLength),
|
||||
kThumbMiddleByteLength)),
|
||||
kThumbMiddleByteLength),
|
||||
toRegisterCount(kThumbPadByteLength)},
|
||||
|
||||
{FingerType::PALM,
|
||||
TactileRegion::PALM_PAD,
|
||||
toRegisterAddress(kTouchSensorBaseByteAddrPalm),
|
||||
kTouchSensorBaseByteAddrPalm,
|
||||
kTouchSensorRegCountPalm}
|
||||
}};
|
||||
|
||||
@ -201,8 +199,9 @@ namespace {
|
||||
const auto& config = kTactileRegionConfigs[i];
|
||||
if (!segments.empty()) {
|
||||
auto& last_segment = segments.back();
|
||||
const int last_end_register = last_segment.start_register + last_segment.register_count;
|
||||
if (last_end_register == config.start_register) {
|
||||
const int last_end_address =
|
||||
last_segment.start_address + last_segment.register_count * kRegisterByteWidth;
|
||||
if (last_end_address == config.start_address) {
|
||||
last_segment.register_count += config.register_count;
|
||||
last_segment.region_indexes.push_back(i);
|
||||
continue;
|
||||
@ -210,7 +209,7 @@ namespace {
|
||||
}
|
||||
|
||||
TactileReadSegment segment;
|
||||
segment.start_register = config.start_register;
|
||||
segment.start_address = config.start_address;
|
||||
segment.register_count = config.register_count;
|
||||
segment.region_indexes.push_back(i);
|
||||
segments.push_back(std::move(segment));
|
||||
@ -281,7 +280,7 @@ void ModbusController::writeRegisters(const int address, const uint16_t* values,
|
||||
}
|
||||
}
|
||||
|
||||
void ModbusController::readRegisterBlock(const int start_addr, const int count, std::vector<uint16_t>& values) {
|
||||
void ModbusController::readRegisterBlock(const int start_address, const int count, std::vector<uint16_t>& values) {
|
||||
values.assign(static_cast<size_t>(count), 0);
|
||||
|
||||
std::lock_guard<std::mutex> lock(io_mutex_);
|
||||
@ -291,15 +290,16 @@ void ModbusController::readRegisterBlock(const int start_addr, const int count,
|
||||
|
||||
for (int offset = 0; offset < count; offset += kMaxRegistersPerRead) {
|
||||
const int current_count = std::min(kMaxRegistersPerRead, count - offset);
|
||||
const int rc = modbus_read_registers(ctx_, start_addr + offset, current_count, values.data() + offset);
|
||||
const int current_address = start_address + offset * kRegisterByteWidth;
|
||||
const int rc = modbus_read_registers(ctx_, current_address, current_count, values.data() + offset);
|
||||
if (rc == -1) {
|
||||
throw std::runtime_error(
|
||||
"Failed to read register block at " + std::to_string(start_addr + offset) + ": " +
|
||||
"Failed to read register block at " + std::to_string(current_address) + ": " +
|
||||
modbus_strerror(errno));
|
||||
}
|
||||
if (rc != current_count) {
|
||||
throw std::runtime_error(
|
||||
"Incomplete register read at " + std::to_string(start_addr + offset) + ", expected " +
|
||||
"Incomplete register read at " + std::to_string(current_address) + ", expected " +
|
||||
std::to_string(current_count) + ", got " + std::to_string(rc));
|
||||
}
|
||||
}
|
||||
@ -428,7 +428,7 @@ void RH56DFTPDexhand::setAngles(const std::vector<int>& finger_joint_angles) {
|
||||
try {
|
||||
ensureConnected();
|
||||
controller_->writeRegisters(
|
||||
toRegisterAddress(kAngleSetByteAddress),
|
||||
kAngleSetByteAddress,
|
||||
registers.data(),
|
||||
static_cast<int>(registers.size()));
|
||||
|
||||
@ -518,10 +518,10 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) {
|
||||
std::vector<uint16_t> segment_values;
|
||||
|
||||
for (const auto& segment : read_plan) {
|
||||
controller_->readRegisterBlock(segment.start_register, segment.register_count, segment_values);
|
||||
controller_->readRegisterBlock(segment.start_address, segment.register_count, segment_values);
|
||||
for (const size_t region_index : segment.region_indexes) {
|
||||
const auto& config = kTactileRegionConfigs[region_index];
|
||||
const int offset = config.start_register - segment.start_register;
|
||||
const int offset = toRegisterCount(config.start_address - segment.start_address);
|
||||
write_buffer.assignRegionData(
|
||||
config.finger,
|
||||
config.region,
|
||||
@ -541,12 +541,14 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) {
|
||||
}
|
||||
|
||||
void RH56DFTPDexhand::tactilePollingLoop() {
|
||||
auto next_poll_deadline = std::chrono::steady_clock::now();
|
||||
std::unique_lock<std::mutex> lock(polling_mutex_);
|
||||
while (tactile_thread_running_.load(std::memory_order_acquire)) {
|
||||
if (requested_polling_mask_.none()) {
|
||||
polling_cv_.wait(lock, [this]() {
|
||||
return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_.any();
|
||||
});
|
||||
next_poll_deadline = std::chrono::steady_clock::now();
|
||||
continue;
|
||||
}
|
||||
|
||||
@ -559,7 +561,14 @@ void RH56DFTPDexhand::tactilePollingLoop() {
|
||||
}
|
||||
lock.lock();
|
||||
|
||||
polling_cv_.wait_for(lock, tactile_poll_interval_, [this, mask]() {
|
||||
next_poll_deadline += tactile_poll_interval_;
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
if (next_poll_deadline <= now) {
|
||||
next_poll_deadline = now;
|
||||
continue;
|
||||
}
|
||||
|
||||
polling_cv_.wait_until(lock, next_poll_deadline, [this, mask]() {
|
||||
return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_ != mask;
|
||||
});
|
||||
}
|
||||
|
||||
@ -61,8 +61,8 @@ TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) {
|
||||
|
||||
ASSERT_NE(hand_config, nullptr);
|
||||
|
||||
const auto finger = DexHand::FingerType::INDEX;
|
||||
const auto region = DexHand::TactileRegion::FINGER;
|
||||
const auto finger = DexHand::FingerType::RING;
|
||||
const auto region = DexHand::TactileRegion::TIP;
|
||||
const int iterations = 20000;
|
||||
const int warmup_ms = 200;
|
||||
const int read_interval_ms = 10;
|
||||
|
||||
@ -118,6 +118,7 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
|
||||
<< ", phase="
|
||||
<< cmvr::app::TouchScreenApp::phaseToString(touch_app_.phase())
|
||||
<< ", active_tag=" << touch_app_.lastActiveTagId()
|
||||
<< ", nonzero_count=" << touch_app_.lastTouchNonzeroCount()
|
||||
<< ", pressure_sum=" << touch_app_.lastTouchPressureSum()
|
||||
<< ", pressure_peak=" << touch_app_.lastTouchPressurePeak();
|
||||
last_logged_status = touch_app_.lastStatus();
|
||||
|
||||
@ -66,7 +66,6 @@ enum TouchScreenTactileRegion {
|
||||
TOUCH_SCREEN_TACTILE_REGION_TIP = 0;
|
||||
TOUCH_SCREEN_TACTILE_REGION_FINGER = 1;
|
||||
TOUCH_SCREEN_TACTILE_REGION_PAD = 2;
|
||||
TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER = 3;
|
||||
TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4;
|
||||
}
|
||||
|
||||
@ -142,4 +141,5 @@ message TouchScreenAppConfig {
|
||||
optional TouchScreenTactileRegion tactile_region = 37;
|
||||
optional double tactile_pressure_sum_threshold = 38;
|
||||
optional double tactile_pressure_peak_threshold = 39;
|
||||
optional int32 tactile_nonzero_count_threshold = 56;
|
||||
}
|
||||
|
||||
243
script/touch_data.py
Normal file
243
script/touch_data.py
Normal file
@ -0,0 +1,243 @@
|
||||
import time
|
||||
from pymodbus.client import ModbusTcpClient
|
||||
from pymodbus.pdu import ExceptionResponse
|
||||
|
||||
# 定义 Modbus TCP 相关参数
|
||||
MODBUS_IP = "192.168.11.210"
|
||||
MODBUS_PORT = 6000
|
||||
|
||||
# 定义各部分数据地址范围
|
||||
TOUCH_SENSOR_BASE_ADDR_PINKY = 3000 # 小拇指
|
||||
TOUCH_SENSOR_END_ADDR_PINKY = 3369
|
||||
|
||||
TOUCH_SENSOR_BASE_ADDR_RING = 3370 # 无名指
|
||||
TOUCH_SENSOR_END_ADDR_RING = 3739
|
||||
|
||||
TOUCH_SENSOR_BASE_ADDR_MIDDLE = 3740 # 中指
|
||||
TOUCH_SENSOR_END_ADDR_MIDDLE = 4109
|
||||
|
||||
TOUCH_SENSOR_BASE_ADDR_INDEX = 4110 # 食指
|
||||
TOUCH_SENSOR_END_ADDR_INDEX = 4479
|
||||
|
||||
TOUCH_SENSOR_BASE_ADDR_THUMB = 4480 # 大拇指
|
||||
TOUCH_SENSOR_END_ADDR_THUMB = 4899
|
||||
|
||||
TOUCH_SENSOR_BASE_ADDR_PALM = 4900 # 掌心
|
||||
TOUCH_SENSOR_END_ADDR_PALM = 5123
|
||||
|
||||
# Modbus 每次最多读取寄存器的数量
|
||||
MAX_REGISTERS_PER_READ = 125
|
||||
|
||||
|
||||
def read_register_range(client, start_addr, end_addr):
|
||||
"""
|
||||
批量读取指定地址范围内的寄存器数据。
|
||||
"""
|
||||
register_values = []
|
||||
# 分段读取寄存器
|
||||
for addr in range(start_addr, end_addr + 1, MAX_REGISTERS_PER_READ * 2):
|
||||
current_count = min(MAX_REGISTERS_PER_READ, (end_addr - addr) // 2 + 1)
|
||||
response = client.read_holding_registers(address=addr, count=current_count)
|
||||
|
||||
if isinstance(response, ExceptionResponse) or response.isError():
|
||||
print(f"读取寄存器 {addr} 失败: {response}")
|
||||
register_values.extend([0] * current_count)
|
||||
else:
|
||||
register_values.extend(response.registers)
|
||||
|
||||
return register_values
|
||||
|
||||
def format_finger_data(finger_name, data):
|
||||
"""
|
||||
格式化四指触觉数据。
|
||||
"""
|
||||
result = {}
|
||||
|
||||
if finger_name != "大拇指":
|
||||
# 四指格式
|
||||
if len(data) < 185:
|
||||
print(f"{finger_name} 数据长度不足，至少185个数据，实际：{len(data)}")
|
||||
return None
|
||||
|
||||
idx = 0
|
||||
# 指端数据 3x3
|
||||
result['tip_end'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)]
|
||||
idx += 9
|
||||
|
||||
# 指尖触觉数据 12x8
|
||||
result['tip_touch'] = [data[idx + i*8: idx + (i+1)*8] for i in range(12)]
|
||||
idx += 96
|
||||
|
||||
# 指腹触觉数据 10x8
|
||||
result['finger_pad'] = [data[idx + i*8: idx + (i+1)*8] for i in range(10)]
|
||||
idx += 80
|
||||
else:
|
||||
# 大拇指格式
|
||||
if len(data) < 210:
|
||||
print(f"{finger_name} 数据长度不足，至少210个数据，实际：{len(data)}")
|
||||
return None
|
||||
|
||||
idx = 0
|
||||
# 指端数据 3x3
|
||||
result['tip_end'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)]
|
||||
idx += 9
|
||||
|
||||
# 指尖触觉数据 12x8
|
||||
result['tip_touch'] = [data[idx + i*8: idx + (i+1)*8] for i in range(12)]
|
||||
idx += 96
|
||||
|
||||
# 指中触觉数据 3x3
|
||||
result['middle_touch'] = [data[idx + i*3: idx + (i+1)*3] for i in range(3)]
|
||||
idx += 9
|
||||
|
||||
# 指腹触觉数据 12x8
|
||||
finger_pad = [data[idx + i*8: idx + (i+1)*8] for i in range(12)]
|
||||
idx += 96
|
||||
|
||||
# 指腹触觉数据行元素反转
|
||||
# finger_pad = [row[::-1] for row in finger_pad]
|
||||
# 指腹触觉数据行顺序反转
|
||||
#finger_pad.reverse()
|
||||
|
||||
result['finger_pad'] = finger_pad
|
||||
|
||||
return result
|
||||
|
||||
def format_palm_data(data):
|
||||
"""
|
||||
格式化掌心数据为14x8矩阵，然后转置为8x14矩阵。
|
||||
"""
|
||||
expected_len = 14 * 8
|
||||
if len(data) < expected_len:
|
||||
print(f"掌心数据长度不足，至少{expected_len}个数据，实际：{len(data)}")
|
||||
return None
|
||||
|
||||
# 生成原始矩阵（14行8列）
|
||||
palm_matrix = [data[i*8:(i+1)*8] for i in range(14)]
|
||||
|
||||
# 转置矩阵
|
||||
transposed = list(map(list, zip(*palm_matrix)))
|
||||
|
||||
return transposed
|
||||
|
||||
|
||||
def print_formatted_finger_data(finger_name, formatted_data):
|
||||
if formatted_data is None:
|
||||
print(f"{finger_name} 数据格式化失败")
|
||||
return
|
||||
|
||||
print(f"--- {finger_name} 指端指端数据 (3x3) ---")
|
||||
for row in formatted_data.get('tip_end', []):
|
||||
print(row)
|
||||
|
||||
print(f"--- {finger_name} 指尖触觉数据 (12x8) ---")
|
||||
for row in formatted_data.get('tip_touch', []):
|
||||
print(row)
|
||||
|
||||
if finger_name == "大拇指":
|
||||
if 'middle_touch' in formatted_data:
|
||||
print(f"--- {finger_name} 指中触觉数据 (3x3) ---")
|
||||
for row in formatted_data['middle_touch']:
|
||||
print(row)
|
||||
else:
|
||||
print(f"{finger_name} 指中触觉数据缺失")
|
||||
|
||||
print(f"--- {finger_name} 指腹触觉数据 ({'12x8' if finger_name == '大拇指' else '10x8'}) ---")
|
||||
for row in formatted_data.get('finger_pad', []):
|
||||
print(row)
|
||||
|
||||
def print_formatted_palm_data(palm_data):
|
||||
if palm_data is None:
|
||||
print("掌心数据格式化失败")
|
||||
return
|
||||
|
||||
print("--- 掌心数据 (8x14) ---")
|
||||
for row in palm_data:
|
||||
print(row)
|
||||
|
||||
def read_firmware_version(client):
|
||||
"""
|
||||
读取固件版本，返回十六进制字符串 地址14
|
||||
"""
|
||||
response = client.read_holding_registers(address=14, count=1)
|
||||
if isinstance(response, ExceptionResponse) or response.isError():
|
||||
print(f"读取固件版本失败：{response}")
|
||||
return None
|
||||
else:
|
||||
reg_value = response.registers[0]
|
||||
# 转换为16进制字符串
|
||||
hex_str = format(reg_value, '04X') # 4位，补零
|
||||
return hex_str
|
||||
|
||||
def read_multiple_registers():
|
||||
client = ModbusTcpClient(MODBUS_IP, port=MODBUS_PORT)
|
||||
client.connect()
|
||||
|
||||
try:
|
||||
while True:
|
||||
start_time = time.time()
|
||||
|
||||
# 读取各部分数据
|
||||
pinky_register_values = read_register_range(
|
||||
client,
|
||||
TOUCH_SENSOR_BASE_ADDR_PINKY,
|
||||
TOUCH_SENSOR_END_ADDR_PINKY
|
||||
)
|
||||
ring_register_values = read_register_range(
|
||||
client,
|
||||
TOUCH_SENSOR_BASE_ADDR_RING,
|
||||
TOUCH_SENSOR_END_ADDR_RING
|
||||
)
|
||||
middle_register_values = read_register_range(
|
||||
client,
|
||||
TOUCH_SENSOR_BASE_ADDR_MIDDLE,
|
||||
TOUCH_SENSOR_END_ADDR_MIDDLE
|
||||
)
|
||||
index_register_values = read_register_range(
|
||||
client,
|
||||
TOUCH_SENSOR_BASE_ADDR_INDEX,
|
||||
TOUCH_SENSOR_END_ADDR_INDEX
|
||||
)
|
||||
thumb_register_values = read_register_range(
|
||||
client,
|
||||
TOUCH_SENSOR_BASE_ADDR_THUMB,
|
||||
TOUCH_SENSOR_END_ADDR_THUMB
|
||||
)
|
||||
palm_register_values = read_register_range(
|
||||
client,
|
||||
TOUCH_SENSOR_BASE_ADDR_PALM,
|
||||
TOUCH_SENSOR_END_ADDR_PALM
|
||||
)
|
||||
|
||||
end_time = time.time()
|
||||
frequency = 1 / (end_time - start_time) if end_time > start_time else float('inf')
|
||||
|
||||
# 格式化数据
|
||||
pinky_formatted = format_finger_data("小拇指", pinky_register_values)
|
||||
ring_formatted = format_finger_data("无名指", ring_register_values)
|
||||
middle_formatted = format_finger_data("中指", middle_register_values)
|
||||
index_formatted = format_finger_data("食指", index_register_values)
|
||||
thumb_formatted = format_finger_data("大拇指", thumb_register_values)
|
||||
palm_formatted = format_palm_data(palm_register_values)
|
||||
|
||||
# 打印格式化数据
|
||||
print_formatted_finger_data("小拇指", pinky_formatted)
|
||||
print_formatted_finger_data("无名指", ring_formatted)
|
||||
print_formatted_finger_data("中指", middle_formatted)
|
||||
print_formatted_finger_data("食指", index_formatted)
|
||||
print_formatted_finger_data("大拇指", thumb_formatted)
|
||||
print_formatted_palm_data(palm_formatted)
|
||||
|
||||
print(f"读取频率：{frequency:.2f} Hz")
|
||||
print("\n" + "="*40 + "\n")
|
||||
|
||||
firmware_version_int = read_firmware_version(client)
|
||||
if firmware_version_int is not None:
|
||||
print(f"固件版本：{firmware_version_int}")
|
||||
|
||||
finally:
|
||||
client.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
read_multiple_registers()
|
||||
Loading…
Reference in New Issue
Block a user