2. 上电流程
本期将介绍如何连接控制器，并根据当前状态安全地完成伺服上电和就绪确认。
通过 SDK 控制机器人之前，需要先连接控制器，再根据当前伺服状态执行清错、切换就绪状态和伺服上电等操作。
伺服状态的含义如下：
- 状态 0：伺服停止，需要先切换到就绪状态
- 状态 1：伺服就绪，可以直接执行上电
- 状态 2：伺服报警，需要先清除报警，再重新上电
- 状态 3：伺服已经上电并进入运行状态
⚠️ 安全警告
伺服上电后，机器人将进入允许运动的状态。执行前必须确认：
- 人员安全： 机器人工作空间内无人员或障碍物
- 急停可用： 急停按钮处于可用状态，并在操作人员可触及范围内
- 控制器状态： 控制器无未处理的安全报警，安全回路工作正常
- 程序检查： 确认后续程序不会立即下发未经验证的运动指令
- 现场配置： 控制器 IP 地址和 SDK 服务端口与实际配置一致
建议先在仿真环境中验证上电流程，确认状态切换符合预期后再连接真机。
1、配置控制器连接和等待参数
首先配置控制器 IP 地址、SDK 服务端口、上电等待超时时间、状态轮询间隔和最大连续查询失败次数。
这些参数集中定义在程序开头，便于根据不同现场环境统一修改，避免相同参数分散在多个函数中，造成遗漏或配置不一致。
cpp
/**
* @name 用户必须根据运行环境修改
* 以下配置决定 SDK 能否连接到实际控制器，运行前必须核对。
* @{
*/
const std::string robot_ip = "192.168.3.243"; ///< 用户实际控制器的 IP 地址
const std::string robot_port = "6001"; ///< 用户实际控制器的 SDK 服务端口，必须与控制器配置一致
/** @} */
/**
* @name 客户需根据本 Demo 修改
* 以下配置关系到上电等待是否符合当前机器人的实际耗时。
* @{
*/
constexpr int ready_timeout_seconds = 90; ///< 等待伺服完成上电的总超时时间
/** @} */
/**
* @name 建议客户修改
* 以下配置用于平衡状态反馈速度与控制器通信负载。
* @{
*/
constexpr int poll_interval_ms = 500; ///< 伺服状态轮询间隔，单位为毫秒
/** @} */
/**
* @name 可修改也可保留默认值
* 默认值适用于一般场景，仅在需要调整通信容错能力时修改。
* @{
*/
constexpr int max_query_failures = 3; ///< 状态查询允许的最大连续失败次数
/** @} */2、封装 SDK 返回值检查函数
由于在上电过程中需要调用多个 SDK 接口，为了帮助判断接口是否调用成功。我们在这里封装 SDK 返回值检查函数，多数 SDK 控制接口都会返回 Result，只有返回 SUCCESS 时，才能确认本次调用成功。
cpp
/**
* @brief 统一检查普通 SDK 接口的返回结果。
* @param[in] result SDK 接口返回的执行结果。
* @param[in] operation SDK 接口名称，用于输出错误信息。
* @retval true SDK 接口调用成功。
* @retval false SDK 接口调用失败。
*/
bool check_sdk_result(Result result, const char* operation)
{
if (result == SUCCESS)
{
return true; // SDK 调用成功。
}
std::cerr << operation << "调用失败，错误码："
<< static_cast<int>(result) << std::endl;
return false; // SDK 调用失败。
}3、封装等待伺服就绪函数
set_servo_poweron 调用成功只表示上电命令已经成功发送，不代表伺服已经立即进入运行状态。控制器完成内部状态切换需要一定时间，因此程序还需要持续查询伺服状态，直到状态变为 3。
cpp
/**
* @brief 等待伺服完成上电并进入运行状态。
* @param[in] fd 控制器连接句柄。
* @param[in] timeout 本次等待允许占用的最长时间。
* @retval true 已确认伺服进入运行状态。
* @retval false 查询连续失败、伺服报警或等待超时。
*/
bool wait_until_robot_ready(
SOCKETFD fd,
std::chrono::seconds timeout = std::chrono::seconds(ready_timeout_seconds))
{
using clock = std::chrono::steady_clock; // 使用单调时钟，避免系统时间变化影响超时判断。
const auto deadline = clock::now() + timeout; // 计算整个等待过程的绝对截止时间。
int state = 0;
int consecutiveFailures = 0; // 记录连续查询失败次数，用于处理短暂通信波动。
while (clock::now() < deadline)
{
const Result result = get_servo_state(fd, state); // 每轮只查询一次伺服状态。
if (result != SUCCESS)
{
++consecutiveFailures;
std::cerr << "获取伺服状态失败，第"
<< consecutiveFailures << "/" << max_query_failures
<< "次，错误码：" << static_cast<int>(result) << std::endl;
if (consecutiveFailures >= max_query_failures)
{
std::cerr << "连续获取伺服状态失败，停止等待" << std::endl;
return false;
}
std::this_thread::sleep_for(
std::chrono::milliseconds(poll_interval_ms));
continue; // 本轮没有获得有效状态，直接开始下一轮查询。
}
consecutiveFailures = 0; // 查询恢复成功后，将连续失败次数清零。
if (state == 3)
{
std::cout << "伺服上电成功，机器人已经进入运行状态" << std::endl;
return true;
}
if (state == 2)
{
std::cerr << "伺服进入报警状态，上电失败" << std::endl;
return false;
}
// 状态为 0 或 1 时，等待一个轮询周期后再次查询。
std::this_thread::sleep_for(
std::chrono::milliseconds(poll_interval_ms));
}
std::cerr << "等待伺服上电超时，最后状态：" << state << std::endl;
return false;
}其中使用 std::chrono::steady_clock 计算超时时间，是为了避免系统时间被人工修改或自动校时后影响等待结果。只有连续查询失败达到上限时才结束流程，能够容忍短暂的通信波动；任意一次查询成功后，连续失败计数都会清零。
4、封装伺服上电函数
机器人可能处于停止、就绪、报警或已经运行等不同状态。上电前必须先读取实际状态，并根据状态选择正确的处理路径。
cpp
/**
* @brief 根据当前伺服状态执行上电流程。
* @param[in] fd 控制器连接句柄。
* @retval true 机器人已处于运行状态，或已完成上电并确认就绪。
* @retval false 状态查询、清错、状态切换、上电或就绪确认失败。
*/
bool power_on(int fd)
{
int state = 0;
if (!check_sdk_result(get_servo_state(fd, state), "get_servo_state"))
return false; // 状态未知时不允许继续发送控制命令。
switch (state)
{
case 0: // 停止状态：先切换到状态 1，再执行上电。
if (!check_sdk_result(set_servo_state(fd, 1), "set_servo_state"))
return false;
if (!check_sdk_result(set_servo_poweron(fd), "set_servo_poweron"))
return false;
break;
case 1: // 就绪状态：可以直接发送伺服上电命令。
if (!check_sdk_result(set_servo_poweron(fd), "set_servo_poweron"))
return false;
break;
case 2: // 报警状态：先清错，再重新设置就绪状态并执行上电。
if (!check_sdk_result(clear_error(fd), "clear_error"))
return false;
std::cerr << "伺服报警已清除，重新上电" << std::endl;
if (!check_sdk_result(set_servo_state(fd, 1), "set_servo_state"))
return false;
if (!check_sdk_result(set_servo_poweron(fd), "set_servo_poweron"))
return false;
break;
case 3: // 伺服已经处于运行状态，无需重复发送上电命令。
return true;
default: // 未定义状态不能安全处理，直接结束上电流程。
std::cerr << "未知伺服状态：" << state << std::endl;
return false;
}
// 上电命令发送成功后，继续等待状态 3，以确认伺服真正就绪。
return wait_until_robot_ready(fd);
}各状态的处理逻辑如下：
- 状态 0：调用
set_servo_state切换到状态 1，再调用set_servo_poweron。 - 状态 1：控制器已经就绪，直接调用
set_servo_poweron。 - 状态 2：先调用
clear_error，清错成功后重新切换到状态 1，再执行上电。 - 状态 3：机器人已经处于运行状态，直接返回成功，避免重复上电。
- 未知状态：无法确定安全处理方式，返回失败并停止后续操作。
5、主程序连接控制器并执行上电
主函数负责启用 UTF-8 控制台输出、连接控制器、调用封装好的上电函数，并根据结果决定程序是否继续。
上电失败时需要主动调用 disconnect_robot 释放已经建立的连接，避免异常路径遗留无效会话。
cpp
#include <iostream>
#include <thread>
#include <chrono>
#include <cpp_interface/nrc_interface.h>
#include "../demo_utils.h"
/**
* @brief Demo 程序入口。
* @return 连接失败时返回 0；上电失败时返回 1；成功时按默认规则返回 0。
*/
int main()
{
demo::enable_console_utf8(); // 启用 UTF-8 控制台输出。
// 同步连接控制器，并通过返回的连接句柄判断连接结果。
SOCKETFD fd = connect_robot(robot_ip, robot_port);
if (fd <= 0)
{
std::cout << "控制器连接失败" << std::endl;
return 0;
}
std::cout << "控制器连接成功" << std::endl;
// 只有确认伺服进入状态 3 后，才继续执行成功路径。
if (!power_on(fd))
{
std::cerr << "伺服未就绪，停止后续操作" << std::endl;
disconnect_robot(fd); // 上电失败时主动释放控制器连接。
return 1;
}
std::cout << "伺服已就绪，可以执行后续操作" << std::endl;
}