关节控制

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

1 单关节控制

1.1 发送单关节角度

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);

2 多关节控制

2.1 获取全部关节角度

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

2.2 发送全部关节角度

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> 

3 完整使用案例

int main(int argc, char* argv[])
try {
    QCoreApplication a(argc, argv);
    using namespace std::chrono_literals;
    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);
    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] << "]";
    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);
    }

    mycobot::MyCobot::I().JogAngle(mycobot::Joint::J1, 1, 5);
    std::this_thread::sleep_for(5000ms);
    mycobot::MyCobot::I().StopRobot();

    std::cout << "\n";
    exit(EXIT_SUCCESS);
} catch (std::error_code&) {
    std::cerr << "System error. Exiting.\n";
    exit(EXIT_FAILURE);
} catch (...) {
    std::cerr << "Unknown exception thrown. Exiting.\n";
    exit(EXIT_FAILURE);
}