纳博特科技开放平台

Appearance

3.平滑的连续轨迹运动 ​

运动模式下，不需要上电，只需要打开运动模式即可，所以在该小节，我们不需要封装上电函数，但是在运动模式下，确保伺服保持就绪状态，而不在停止状态。

1、封装设置伺服就绪函数 ​

cpp
/*
 * 伺服就绪函数
 */
void servo_ready(int fd)
{
    int state = 0;
    get_servo_state(fd, state);
    switch (state)
    {
    case 0:
        set_servo_state(fd, 1); // 将伺服切换为就绪
        break;
    case 2:
        clear_error(fd);
        set_servo_state(fd, 1);
        break;
    }
    get_servo_state(fd, state);
    std::cout << "伺服状态: " << state << std::endl;
}

2、封装等待运动结束的函数 ​

cpp
/*
 * 循环阻塞运动结束函数
 */
void wait_for_running_over(int fd)
{
    // 等待运动完成
    int running_state = 0;
    get_robot_running_state(fd, running_state);  // 查询机器人是否在运动 2-正在运动
    while (running_state == 2)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(500));  // 阻塞500ms
        get_robot_running_state(fd, running_state);   // 再次查询
    }
}

3、主要运动功能 ​

cpp
#include <iostream>
#include <thread>
#include <chrono>
#include "cpp_interface/nrc_api.h"  // 导入头文件


int main()
{
    SOCKETFD fd = connect_robot("192.168.1.15", "6001");
    if (fd <= 0)
    {
        std::cout << "连接失败" << std::endl;
        return 0;
    }
    std::cout << "连接成功: "<< fd << std::endl;
    
    set_current_mode(fd, 2);  // 设置运动模式
    set_speed(fd, 50); // 设置运动模式下的全局速度


    servo_ready(fd);   // 调用封装的伺服就绪函数


    queue_motion_set_status(fd, 1); // 打开队列


    // 构建运动指令，开始运动
    MoveCmd temp_cmd;
    temp_cmd.targetPosType = PosType::data;
    get_current_position(fd, 0, temp_cmd.targetPosValue);  // 获取当前的关节坐标
    temp_cmd.targetPosValue[0] += 10;  // 基于当前位置 1轴向正方向移动10°
    temp_cmd.coord = 0;  // 关节坐标系下移动
    temp_cmd.velocity = 50;  // 指令速度
    temp_cmd.acc = 100;
    temp_cmd.dec = 100;
    temp_cmd.pl = 5;  // 队列轨迹之间的平滑度
    queue_motion_push_back_moveJ(fd, temp_cmd);
    temp_cmd.targetPosValue[1] += 10;
    queue_motion_push_back_moveJ(fd, temp_cmd);
    temp_cmd.coord = 1;
    temp_cmd.velocity = 500;  // 指令速度 mm/s
    temp_cmd.acc = 100;
    temp_cmd.dec = 100;
    temp_cmd.pl = 5;
    get_current_position(fd, 1, temp_cmd.targetPosValue);  // 获取当前的直角坐标
    temp_cmd.targetPosValue[0] += 20;  // 基于当前位置 X向正方向移动20mm
    queue_motion_push_back_moveL(fd, temp_cmd);


    queue_motion_send_to_controller(fd, 3);  // 将上面三个队列发送到控制器


    // 等待运动完成
    wait_for_running_over(fd);


    // 查询运动后坐标
    std::vector<double> pos(7);
    get_current_position(fd, 0, pos);
    std::cout << "关节运动后坐标：" << pos[0] << " " << pos[1] << " " << pos[2] << " " << pos[3] << " " << pos[4] << " " << pos[5] << " " << pos[6] << std::endl;


    queue_motion_set_status(fd, 0); // 关闭队列
    set_current_mode(fd, 0); // 切换回示教模式


    return 0;
}