3. Smooth Continuous Trajectory Motion

In run mode, power-on is not required; you only need to enable motion mode. Therefore, in this section we do not need to encapsulate a power-on function. However, in run mode, ensure the servo remains in the ready state, not the stopped state.

1. Encapsulate the Servo Ready Function

/*
 * Servo ready function
 */
void servo_ready(int fd)
{
    int state = 0;
    get_servo_state(fd, state);
    switch (state)
    {
    case 0:
        set_servo_state(fd, 1); // Set servo to ready
        break;
    case 2:
        clear_error(fd);
        set_servo_state(fd, 1);
        break;
    }
    get_servo_state(fd, state);
    std::cout << "Servo state: " << state << std::endl;
}

2. Encapsulate the Wait-for-Motion-End Function

/*
 * Loop-blocking motion end function
 */
void wait_for_running_over(int fd)
{
    // Wait for motion to complete
    int running_state = 0;
    get_robot_running_state(fd, running_state);  // Query whether the robot is moving, 2 = moving
    while (running_state == 2)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(500));  // Block for 500ms
        get_robot_running_state(fd, running_state);   // Query again
    }
}

3. Main Motion Function

#include <iostream>
#include <thread>
#include <chrono>
#include "cpp_interface/nrc_api.h"  // Import header file


int main()
{
    SOCKETFD fd = connect_robot("192.168.1.15", "6001");
    if (fd <= 0)
    {
        std::cout << "Connection failed" << std::endl;
        return 0;
    }
    std::cout << "Connection succeeded: "<< fd << std::endl;
    
    set_current_mode(fd, 2);  // Set run mode
    set_speed(fd, 50); // Set global speed in run mode


    servo_ready(fd);   // Call the encapsulated servo ready function


    queue_motion_set_status(fd, 1); // Enable the queue


    // Build motion commands, start moving
    MoveCmd temp_cmd;
    temp_cmd.targetPosType = PosType::data;
    get_current_position(fd, 0, temp_cmd.targetPosValue);  // Get current joint coordinates
    temp_cmd.targetPosValue[0] += 10;  // Move axis 1 +10 degrees forward from current position
    temp_cmd.coord = 0;  // Move in joint coordinate system
    temp_cmd.velocity = 50;  // Command velocity
    temp_cmd.acc = 100;
    temp_cmd.dec = 100;
    temp_cmd.pl = 5;  // Smoothness between queue trajectories
    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;  // Command velocity in mm/s
    temp_cmd.acc = 100;
    temp_cmd.dec = 100;
    temp_cmd.pl = 5;
    get_current_position(fd, 1, temp_cmd.targetPosValue);  // Get current Cartesian coordinates
    temp_cmd.targetPosValue[0] += 20;  // Move X axis +20mm forward from current position
    queue_motion_push_back_moveL(fd, temp_cmd);


    queue_motion_send_to_controller(fd, 3);  // Send the three queues above to the controller


    // Wait for motion to complete
    wait_for_running_over(fd);


    // Query position after motion
    std::vector<double> pos(7);
    get_current_position(fd, 0, pos);
    std::cout << "Joint position after motion: " << pos[0] << " " << pos[1] << " " << pos[2] << " " << pos[3] << " " << pos[4] << " " << pos[5] << " " << pos[6] << std::endl;


    queue_motion_set_status(fd, 0); // Disable the queue
    set_current_mode(fd, 0); // Switch back to teach mode


    return 0;
}