2. Single-Point Motion without Smoothing

Our controller has three modes: teach mode, run mode, and remote mode.

Teach mode further includes: drag teach and jog teach.

This section introduces how to perform single-point motion in jog teach mode.

1. Encapsulate the Power-On Function

Single-point motion is performed in jog teach mode. However, motion in teach mode requires an additional power-on step, so we need to encapsulate a power-on function.

/*
 * Encapsulated power-on function
 */
void power_on(int fd)
{
    int state = 0;
    get_servo_state(fd, state);  // Query the current servo state
    switch (state)
    {
    case 0: // Current servo is stopped
        set_servo_state(fd, 1);  // Set servo to ready
        set_servo_poweron(fd);   // Power on the servo
        break;
    case 1: // Current servo is ready
        set_servo_poweron(fd);
        break;
    case 2: // Current servo is in alarm
        clear_error(fd);         // Clear error
        set_servo_state(fd, 1);
        set_servo_poweron(fd);
        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

A delay function will be used, requiring the additional inclusion of <thread> and <chrono> header files.

#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");  // Connect to the controller
    if (fd <= 0)
    {
        std::cout << "Connection failed" << std::endl;
        return 0;
    }
    std::cout << "Connection succeeded: "<< fd << std::endl;

    set_current_mode(fd, 0);   // Set teach mode. Only motion in teach mode requires power-on
    set_speed(fd, 50);         // Set global speed in teach mode


    power_on(fd);              // Call the encapsulated power-on function


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


    // Build motion command, start moving
    MoveCmd temp_cmd;
    temp_cmd.coord = 0;                                    // Set joint coordinate system
    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.velocity = 50;                                // Command velocity. Speed range in joint coordinates: [1, 100]
    temp_cmd.acc = 100;                                    // Command acceleration
    temp_cmd.dec = 100;                                    // Command deceleration
    robot_movej(fd, temp_cmd);


    std::this_thread::sleep_for(std::chrono::milliseconds(200));  // Block for 200ms
    wait_for_running_over(fd);   // Block until motion ends


    // Query position after motion
    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;


    return 0;
}