关节控制（RobotArm）
机械臂的关节控制指对各关节变量下发目标角或读取当前角，并在给定速度约束下运动到目标。本文档基于 RobotArm（include/RobotArm.hpp），适用于 MyCobotPro450；运行环境为 Linux，通信为 TCP（见 环境搭建、编译）。
下文接口均按 功能、返回值、参数说明、取值范围 列出。完整 API 索引见 API 说明。
关节角取值（°）
| 关节 id | 角度范围 |
|---|---|
| 1 | -162 ~ 162 |
| 2 | -125 ~ 125 |
| 3 | -154 ~ 154 |
| 4 | -162 ~ 162 |
| 5 | -162 ~ 162 |
| 6 | -165 ~ 165 |
速度档 sp / speed：1 ~ 100（全篇同）。
单关节
int sendAngle(uint8_t joint, float angle, uint8_t sp)
- 功能：向指定关节下发目标角度。
- 返回值：通信与指令成功时多为
1，失败多为-1（以下位机为准）。 - 参数说明：
joint关节序号；angle目标角（°）；sp速度档。 - 取值范围：
joint：1 ~ 6；angle：见上表；sp：1 ~ 100。
多关节
float getAngle(uint8_t joint)
- 功能：读取单关节当前角度。
- 返回值：浮点角度（°）；异常时可能为哨兵值（以联调为准）。
- 参数说明：
joint。 - 取值范围：
joint：1 ~ 6。
Angles getAngles()
- 功能：一次读取六关节当前角度。
- 返回值：
Angles（std::array<float, 6>），下标 0 ~ 5 对应 J1 ~ J6。 - 参数说明：无。
- 取值范围：无。
int sendAngles(Angles angles, uint8_t sp)
- 功能：一次下发六关节目标角度。
- 返回值：成功多为
1，失败-1。 - 参数说明：
angles六个目标角；sp速度档。 - 取值范围：各元素 按关节1~6 对应上表；
sp：1 ~ 100。
float getJointMinAngle(uint8_t joint) / float getJointMaxAngle(uint8_t joint)
- 功能：读取软件关节限位（最小/最大角）。
- 返回值：浮点（°）。
- 参数说明：
joint。 - 取值范围：
joint：1 ~ 6。
JOG 与辅助
int jogAngle(uint8_t joint, uint8_t direction, uint8_t speed)
- 功能：指定关节沿某方向连续转动，直至限位或调用
stop()等停止。 - 返回值：成功多为
1，失败-1。 - 参数说明：
joint；direction；speed。 - 取值范围：
joint：1 ~ 6；direction：0 负向、1 正向；speed：1 ~ 100。
int setFreeMoveMode(bool state) / int getFreeMoveMode()
- 功能：打开/关闭自由移动模式（便于手动拖动等）；或查询当前状态。
- 返回值：设置接口成功多为
1；查询为0关闭、1打开，失败-1。 - 参数说明：
state，false/true。 - 取值范围：逻辑值对应 0 / 1。
int setMotorEnabled(uint8_t joint, bool state) / int servoRestore(uint8_t joint)
- 功能：单关节使能/掉使能；或清除关节异常（恢复类指令）。
- 返回值：成功多为
1，失败-1。 - 参数说明：
joint；setMotorEnabled另需state（false掉使能，true使能）。 - 取值范围：
joint：1 ~ 6 或 254（全部关节）。
到位与闭环（与关节指令配合）
int isInPosition(Angles angle_coords, bool mode)
- 功能：判断当前是否已到达给定六轴目标。
- 返回值：
1到位；0未到位；-1异常或无有效应答。 - 参数说明：
angle_coords六个目标量；mode为数据类型。 - 取值范围：判断关节角时取
mode == false；各角 见本节关节角表。
void setWaitArrival(bool enable) / bool getWaitArrival() const / bool waitPosArrived(int timeout_ms = 30000)
- 功能：设置是否在运动指令后等待到位再返回；查询该开关；或阻塞等待到位反馈。
- 返回值：
setWaitArrival无返回值；getWaitArrival为bool；waitPosArrived等到为true，超时等为false。 - 参数说明：
enable；timeout_ms超时（毫秒）。 - 取值范围：
timeout_ms> 0，默认 30000。
拖动示教
int dragTeachSave() / int dragTeachPause() / int dragTeachExecute() / int dragTeachClean()
- 功能：开始录制拖动轨迹；暂停录制；执行已录轨迹一次；清除采样点。
- 返回值：成功多为
1，失败-1。 - 参数说明：无。
- 取值范围：单次录制建议 ≤ 120 s。
示例（节选）
以下示例假设已能 TCP 连接机械臂侧服务，仅演示关节相关调用思路；错误处理与线程安全请按工程要求补充。
#include "RobotArm.hpp"
#include "config.hpp"
#include <iostream>
#include <thread>
#include <chrono>
using namespace std::chrono_literals;
int main() {
Param p;
p.ip_serial = "192.168.0.232";
p.port_baud = 4500;
RobotArm arm(TCP_COMM, p);
if (arm.powerOn() != 1) {
std::cerr << "powerOn failed\n";
return 1;
}
Angles cur = arm.getAngles();
std::cout << "J1=" << cur[0] << " ...\n";
Angles goal = {5.f, 5.f, 5.f, -5.f, 5.f, 5.f};
if (arm.sendAngles(goal, 30) != 1) {
std::cerr << "sendAngles failed\n";
}
while (arm.isInPosition(goal, false) != 1) {
cur = arm.getAngles();
std::cout << "J1=" << cur[0] << "\n";
std::this_thread::sleep_for(200ms);
}
arm.jogAngle(1, 1, 30);
std::this_thread::sleep_for(2s);
arm.stop();
arm.powerOff();
return 0;
}