forked from Guorong/inspiredhand
103 lines
3.9 KiB
C++
103 lines
3.9 KiB
C++
#include <cstdio>
|
|
#include <string>
|
|
|
|
#include "jaka_robot/robot_action.h"
|
|
#include "jaka_robot/robot_controller.h"
|
|
#include "inspire_hand/hand_actions.h"
|
|
#include "tools/config_loader.h"
|
|
#include "tools/tools.h"
|
|
|
|
using tools::sleep_ms;
|
|
|
|
int main()
|
|
{
|
|
// 1) 读取默认配置（机器人 IP + TIO 参数）
|
|
const config::AppConfig &cfg = config::get_default_app_config();
|
|
const std::string ip = cfg.robot_ip;
|
|
|
|
// 2) 通过通用的 init_robot / init_tio 逻辑完成机械臂与 TIO 初始化
|
|
RobotController rc;
|
|
robot_action::RobotParams params{90.0, 200.0, 400.0, 250.0, 180.0, 400.0};
|
|
|
|
if (!robot_action::init_robot(rc, ip, params)) {
|
|
return -1;
|
|
}
|
|
if (!rc.init_tio_for_hand()) {
|
|
return -1;
|
|
}
|
|
|
|
std::printf("\n==== RH56 FORCE_ACT 测试：持续读取力度信号量 ====\n");
|
|
std::printf("按回车键开始力度测试...\n");
|
|
tools::get_line_trimmed();
|
|
|
|
// 4) 循环读取信号量，打印当前力度值
|
|
for (int sample = 1; sample <= 12; ++sample) {
|
|
auto forces = hand_actions::get_force_service();
|
|
std::printf("[FORCE] sample %02d : ", sample);
|
|
for (size_t i = 0; i < forces.size(); ++i) {
|
|
std::printf("F%zu=%d ", i, static_cast<int>(forces[i]));
|
|
}
|
|
std::printf("\n");
|
|
sleep_ms(200); // 200ms 打印一次
|
|
}
|
|
|
|
std::printf("\n==== RH56 ANGLE_ACT 测试：在第 3/6/9 步下发不同角度命令 ====\n");
|
|
|
|
// 5) 循环读取信号量，打印当前角度值
|
|
for (int step = 1; step <= 12; ++step) {
|
|
auto angles = hand_actions::get_angle_act_service();
|
|
std::printf("[ANGLE] step %02d : ", step);
|
|
for (size_t i = 0; i < angles.size(); ++i) {
|
|
std::printf("A%zu=%d ", i, static_cast<int>(angles[i]));
|
|
}
|
|
std::printf("\n");
|
|
|
|
// 在第 3、6、9 次循环时，分别下发不同的角度命令，测试 TIO 发送 + 信号量读取
|
|
if (step == 3) {
|
|
std::printf("[ANGLE] STEP %d -> set_angle_service: 全展开 (1000,1000,1000,1000,1000,1000)\n", step);
|
|
hand_actions::set_angle_service(1000,1000,1000,1000,1000,1000);
|
|
} else if (step == 6) {
|
|
std::printf("[ANGLE] STEP %d -> set_angle_service: 握拳 (0,0,0,0,200,1000)\n", step);
|
|
hand_actions::set_angle_service(0,0,0,0,200,1000);
|
|
} else if (step == 9) {
|
|
std::printf("[ANGLE] STEP %d -> set_angle_service: 部分弯曲 (1000,1000,1000,500,550,150)\n", step);
|
|
hand_actions::set_angle_service(1000,1000,1000,500,550,150);
|
|
}
|
|
|
|
sleep_ms(200); // 200ms 打印一次
|
|
}
|
|
|
|
std::printf("\n==== RH56 SPEED_SET 测试：在第 3/6/9 步下发不同速度命令 ====\n");
|
|
|
|
// 6) 循环读取信号量，打印当前速度值
|
|
for (int step = 1; step <= 12; ++step) {
|
|
auto speeds = hand_actions::get_speed_set_service();
|
|
std::printf("[SPEED] step %02d : ", step);
|
|
for (size_t i = 0; i < speeds.size(); ++i) {
|
|
std::printf("S%zu=%d ", i, static_cast<int>(speeds[i]));
|
|
}
|
|
std::printf("\n");
|
|
|
|
// 在第 3、6、9 次循环时，下发不同速度，测试 TIO 写入是否能在信号量中正确反映
|
|
if (step == 3) {
|
|
std::printf("[SPEED] STEP %d -> set_speed_service: 低速(200)\n", step);
|
|
hand_actions::set_speed_service(200,200,200,200,200,200);
|
|
} else if (step == 6) {
|
|
std::printf("[SPEED] STEP %d -> set_speed_service: 中速(400)\n", step);
|
|
hand_actions::set_speed_service(400,400,400,400,400,400);
|
|
} else if (step == 9) {
|
|
std::printf("[SPEED] STEP %d -> set_speed_service: 高速(800)\n", step);
|
|
hand_actions::set_speed_service(800,800,800,800,800,800);
|
|
}
|
|
|
|
sleep_ms(200); // 200ms 打印一次
|
|
}
|
|
|
|
// 若上面循环改为有限次，可在退出前做清理
|
|
// robot.disable_robot();
|
|
// robot.power_off();
|
|
// robot.login_out();
|
|
|
|
return 0;
|
|
}
|