forked from Guorong/inspiredhand
103 lines
2.9 KiB
C++
103 lines
2.9 KiB
C++
#include <cstdio>
|
||
#include <cstdint>
|
||
#include <array>
|
||
#include <string>
|
||
#include <thread>
|
||
#include <chrono>
|
||
|
||
#include "JAKAZuRobot.h" // comes from c&c++/inc_of_c++
|
||
#include "inspire_hand_driver/rh56_frame.h"
|
||
#include "tools/tools.h"
|
||
|
||
using tools::sleep_ms;
|
||
|
||
int main()
|
||
{
|
||
const std::string ip = "10.5.5.100"; // adjust to your controller IP
|
||
const int rs485_channel = 1; // TIO RS485L channel id
|
||
const uint8_t hand_id = 1; // RH56 hand id , unused here because of only one
|
||
|
||
JAKAZuRobot robot;
|
||
if (robot.login_in(ip.c_str()) != ERR_SUCC) {
|
||
std::printf("login_in failed\n");
|
||
return 1;
|
||
}
|
||
robot.power_on();
|
||
robot.enable_robot();
|
||
|
||
// 1) 设置TIO 24V供电
|
||
// vout_enable=1 启用，vout_vol=0 设置为24V
|
||
if (robot.set_tio_vout_param(1, 0) != ERR_SUCC) {
|
||
std::printf("set_tio_vout_param failed\n");
|
||
robot.login_out();
|
||
return 1;
|
||
}
|
||
|
||
// 2) 设置TIO为RS485L模式
|
||
robot.set_tio_pin_mode(2, 0x01); // AI pins enable RS485L
|
||
|
||
|
||
// 3) 设置RS485通道的Modbus RTU通讯参数
|
||
ModRtuComm modcfg{};
|
||
modcfg.chn_id = 1; // 485L
|
||
modcfg.slaveId = 2;
|
||
modcfg.baudrate = 115200;
|
||
modcfg.databit = 8;
|
||
modcfg.stopbit = 1;
|
||
modcfg.parity = 78; // 无校验
|
||
if (robot.set_rs485_chn_comm(modcfg) != ERR_SUCC) {
|
||
std::printf("set_rs485_chn_comm failed\n");
|
||
robot.login_out();
|
||
return 1;
|
||
}
|
||
|
||
PayLoad payloadset;
|
||
robot.get_payload(&payloadset);
|
||
std::printf("mass=%f\n",payloadset.mass);
|
||
std::printf("center=%f %f %f\n",payloadset.centroid.x,payloadset.centroid.y,payloadset.centroid.z);
|
||
|
||
|
||
// Build and send frames via RH56 frame-construction API
|
||
uint8_t tx_buf[64];
|
||
size_t tx_len = 0;
|
||
|
||
|
||
// Set speeds
|
||
std::array<uint16_t,6> speeds = {200,200,200,200,200,200};
|
||
rh56_result_t r = rh56_make_set_speeds(speeds.data(), tx_buf, &tx_len);
|
||
if (r != RH56_OK) {
|
||
std::printf("make_set_speeds failed: %d\n", r);
|
||
robot.login_out();
|
||
return 1;
|
||
}
|
||
robot.send_tio_rs_command(rs485_channel, tx_buf, (int)tx_len);
|
||
|
||
sleep_ms(50);
|
||
|
||
// Set angles
|
||
std::array<int16_t,6> angles = {1000,1000,1000,1000,1000,1000};
|
||
r = rh56_make_set_angles(angles.data(), tx_buf, &tx_len);
|
||
if (r != RH56_OK) {
|
||
std::printf("make_set_angles failed: %d\n", r);
|
||
robot.login_out();
|
||
return 1;
|
||
}
|
||
robot.send_tio_rs_command(rs485_channel, tx_buf, (int)tx_len);
|
||
|
||
sleep_ms(100);
|
||
|
||
// Read angles (construct read frame and send; receiving path not handled here)
|
||
r = rh56_make_read_angles(tx_buf, &tx_len);
|
||
if (r != RH56_OK) {
|
||
std::printf("make_read_angles failed: %d\n", r);
|
||
robot.login_out();
|
||
return 1;
|
||
}
|
||
robot.send_tio_rs_command(rs485_channel, tx_buf, (int)tx_len);
|
||
|
||
/*robot.disable_robot();
|
||
robot.power_off();
|
||
robot.login_out();*/
|
||
return 0;
|
||
}
|