关节控制

对于串联式多关节机器人，关节控制是针对机械臂各个关节的变量进行的控制，目标是让机械臂各个关节按照一定速度达到目标位置。

单关节控制

发送单关节角度

WriteAngle(Joint joint, double value, int speed = DefaultSpeed)
返回值：无
参数说明：参数1：关节号（1-6） 参数2：角度(-170°- 170°) 参数3：速度(0-100)，默认为30
示例：
mycobot::MyCobot::I().WriteAngle(mycobot::Joint::J1, 10, 30);

多关节控制

获取全部关节角度

GetAngles()
返回值：Angles类型
参数说明：无
示例：
mycobot::Angles angles= mycobot::MyCobot::I().GetAngles();

发送全部关节角度

WriteAngles(const Angles& angles, int speed = DefaultSpeed)
返回值：无
参数说明：参数1：全部角度（std::array,角度取值范围-170°- 170°） 参数2：速度(0-100)，默认为30
示例：

mycobot::Angles goal_angles = { 5, 5, 5, 5, 5, 5 };<br>
mycobot::MyCobot::I().WriteAngles(goal_angles, 30);<br> 

完整使用案例

下面这段代码是一个控制 MyCobot 机械臂的程序，它负责检查控制器连接、启动和停止机械臂、获取机械臂状态、设置关节角度并在出现异常时处理错误。

int main(int argc, char* argv[])
try {
    QCoreApplication a(argc, argv); // 创建 QCoreApplication 对象，用于支持 Qt 事件循环

    using namespace std::chrono_literals; // 引入时间单位字面量（如 200ms）

    // 检查机械臂控制器是否连接
    if (!mycobot::MyCobot::I().IsControllerConnected()) {
        std::cerr << "Robot is not connected\n"; // 如果未连接，输出错误信息
        exit(EXIT_FAILURE); // 退出程序，返回失败状态
    }

    std::cout << "Robot is connected\n"; // 输出连接成功的信息

    mycobot::MyCobot::I().PowerOn(); // 上电，启动机械臂
    mycobot::MyCobot::I().StopRobot(); // 停止机械臂的所有动作

    // 检查机械臂是否正在移动，并输出状态
    std::cout << "Robot is moving: " << mycobot::MyCobot::I().IsMoving() << "\n";

    // 获取当前机械臂各关节的角度
    mycobot::Angles angles = mycobot::MyCobot::I().GetAngles();
    std::this_thread::sleep_for(200ms); // 延迟 200 毫秒

    // 获取当前机械臂末端的位置坐标
    mycobot::Coords coords = mycobot::MyCobot::I().GetCoords();

    // 再次获取各关节的角度
    angles = mycobot::MyCobot::I().GetAngles();

    // 输出各关节的角度
    std::cout << "[" << angles[mycobot::J1] << ", " << angles[mycobot::J2] << ", "
              << angles[mycobot::J3] << ", " << angles[mycobot::J4] << ", "
              << angles[mycobot::J5] << ", " << angles[mycobot::J6] << "]";

    // 定义一个目标角度数组，所有关节移动到 5 度
    mycobot::Angles goal_angles = { 5, 5, 5, 5, 5, 5 };
    mycobot::MyCobot::I().WriteAngles(goal_angles); // 将机械臂移动到目标角度

    // 等待机械臂移动到目标位置
    while (!mycobot::MyCobot::I().IsInPosition(goal_angles, false)) {
        angles = mycobot::MyCobot::I().GetAngles(); // 获取当前角度
        std::cout << "[" << angles[mycobot::J1] << ", " << angles[mycobot::J2] << ", "
                  << angles[mycobot::J3] << ", " << angles[mycobot::J4] << ", "
                  << angles[mycobot::J5] << ", " << angles[mycobot::J6] << "]" << std::flush;
        std::this_thread::sleep_for(200ms); // 每次循环延迟 200 毫秒
    }

    // 调整 J1 关节角度，增量为 1 度，速度为 5
    mycobot::MyCobot::I().JogAngle(mycobot::Joint::J1, 1, 5);
    std::this_thread::sleep_for(5000ms); // 延迟 5 秒

    mycobot::MyCobot::I().StopRobot(); // 停止机械臂的所有动作

    std::cout << "\n"; // 输出一个换行符
    exit(EXIT_SUCCESS); // 正常退出程序

} catch (std::error_code&) {
    // 捕获 std::error_code 异常，并输出错误信息
    std::cerr << "System error. Exiting.\n";
    exit(EXIT_FAILURE); // 异常退出程序
} catch (...) {
    // 捕获所有其他异常，并输出错误信息
    std::cerr << "Unknown exception thrown. Exiting.\n";
    exit(EXIT_FAILURE); // 异常退出程序
}