134 lines
4.4 KiB
C++
134 lines
4.4 KiB
C++
#include <vector>
|
||
#include <cstdint>
|
||
#include <cstddef>
|
||
#include <array>
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
|
||
#include "robot_control_hand/semophore.h"
|
||
|
||
// 说明：
|
||
// 本文件负责通过 JAKA 控制柜的 TIO RS485 "信号量" 机制
|
||
// 来获取灵巧手的力度、实际角度、设定角度等信息。
|
||
|
||
// 与 communication_bridge.cpp 共用同一个机器人实例：
|
||
// RobotController::init_tio_for_hand() 中会调用 init_semaphore(demo_)
|
||
// 在这里缓存指针，供后续 get_*_service 使用
|
||
// 是读灵巧手信息接口的具体实现
|
||
|
||
static JAKAZuRobot *g_robot_semo = nullptr;
|
||
static int g_rs485_channel_semo = 1;
|
||
|
||
bool init_semaphore(JAKAZuRobot &robot)
|
||
{
|
||
// 从全局缓存获取 TIO 配置
|
||
const config::AppConfig &cfg = config::get_default_app_config();
|
||
|
||
g_robot_semo = &robot;
|
||
g_rs485_channel_semo = cfg.tio.rs485_channel;
|
||
|
||
// 注意：JAKA 控制柜当前版本最多只允许 8 个信号量
|
||
// 这里仅预先建立 6 个力度信号量：FORCE_ACT0..5
|
||
bool ok = true;
|
||
for (int i = 0; i < 6; ++i) {
|
||
SignInfo sig{};
|
||
std::memset(&sig, 0, sizeof(sig));
|
||
std::snprintf(sig.sig_name, sizeof(sig.sig_name), "FORCE_ACT%d", i);
|
||
sig.chn_id = g_rs485_channel_semo; // RS485 通道 ID
|
||
sig.sig_type = 3; // 3: 保持寄存器
|
||
sig.sig_addr = RH56_REG_FORCE_ACT + 2 * i; // 每个关节占 2 bytes
|
||
sig.frequency = 5; // 5Hz 刷新
|
||
|
||
errno_t ret = g_robot_semo->add_tio_rs_signal(sig);
|
||
if (ret != ERR_SUCC) {
|
||
std::printf("add_tio_rs_signal(%s) failed: %d\n", sig.sig_name, ret);
|
||
ok = false;
|
||
}
|
||
//else {
|
||
//std::printf("add_tio_rs_signal ok: %s, addr=%d\n", sig.sig_name, sig.sig_addr);
|
||
//}
|
||
}
|
||
|
||
return ok;
|
||
}
|
||
|
||
namespace hand_actions {
|
||
|
||
//信号量一直在，查找即可
|
||
std::vector<int16_t> get_force_service()
|
||
{
|
||
std::vector<int16_t> forces(6, 0);
|
||
if (!g_robot_semo) return forces;
|
||
|
||
SignInfo infos[64];
|
||
int count = static_cast<int>(sizeof(infos) / sizeof(infos[0]));
|
||
errno_t ret = g_robot_semo->get_rs485_signal_info(infos, &count);
|
||
if (ret != ERR_SUCC) return forces;
|
||
|
||
const char *prefix = "FORCE_ACT";
|
||
const size_t prefix_len = std::strlen(prefix);
|
||
for (int i = 0; i < count; ++i) {
|
||
if (std::strncmp(infos[i].sig_name, prefix, prefix_len) == 0) {
|
||
int idx = infos[i].sig_name[prefix_len] - '0';
|
||
if (idx >= 0 && idx < 6) {
|
||
forces[idx] = static_cast<int16_t>(infos[i].value);
|
||
}
|
||
}
|
||
}
|
||
return forces;
|
||
}
|
||
|
||
// 公共实现：由于 8 个信号量上限，某些寄存器按需“创建 -> 读取 -> 删除”
|
||
static std::vector<int16_t> get_temp_register_values(const char *base_name, int base_reg)
|
||
{
|
||
std::vector<int16_t> values(6, 0);
|
||
if (!g_robot_semo) return values;
|
||
|
||
for (int i = 0; i < 6; ++i) {
|
||
// 1. 创建 base_namei
|
||
SignInfo sig{};
|
||
std::memset(&sig, 0, sizeof(sig));
|
||
std::snprintf(sig.sig_name, sizeof(sig.sig_name), "%s%d", base_name, i);
|
||
sig.chn_id = g_rs485_channel_semo;
|
||
sig.sig_type = 3;
|
||
sig.sig_addr = base_reg + 2 * i;
|
||
sig.frequency = 5;
|
||
g_robot_semo->add_tio_rs_signal(sig);
|
||
|
||
// 2. 读取当前所有信号，从中找到 base_namei
|
||
SignInfo infos[64];
|
||
int count = static_cast<int>(sizeof(infos) / sizeof(infos[0]));
|
||
errno_t ret = g_robot_semo->get_rs485_signal_info(infos, &count);
|
||
if (ret == ERR_SUCC) {
|
||
char target_name[32];
|
||
std::snprintf(target_name, sizeof(target_name), "%s%d", base_name, i);
|
||
for (int k = 0; k < count; ++k) {
|
||
if (std::strcmp(infos[k].sig_name, target_name) == 0) {
|
||
values[i] = static_cast<int16_t>(infos[k].value);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 3. 立刻删除 base_namei
|
||
char name[32];
|
||
std::snprintf(name, sizeof(name), "%s%d", base_name, i);
|
||
g_robot_semo->del_tio_rs_signal(name);
|
||
}
|
||
|
||
return values;
|
||
}
|
||
|
||
// 由于 8 个信号量的上限，以及当前角度、速度不常用，这些信号量用完即删
|
||
std::vector<int16_t> get_angle_act_service()
|
||
{
|
||
return get_temp_register_values("ANGLE_ACT", RH56_REG_ANGLE_ACT);
|
||
}
|
||
|
||
std::vector<int16_t> get_speed_set_service()
|
||
{
|
||
return get_temp_register_values("SPEED_SET", RH56_REG_SPEED_SET);
|
||
}
|
||
|
||
} // namespace hand_actions
|