3. 평활한 연속 궤적 모션

모션 모드에서는 전원 켜기이 필요하지 않고 모션 모드만 열면 됩니다. 따라서 이 절에서는 전원 켜기 함수를 캡슐화할 필요가 없지만, 모션 모드에서는 서보가 정지 상태가 아닌 준비 상태를 유지하도록 해야 합니다.

1. 서보 준비 함수 캡슐화

/*
 * 서보 준비 함수
 */
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. 모션 종료 대기 함수 캡슐화

/*
 * 모션 종료 시까지 루프 블로킹 함수
 */
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. 주요 모션 기능

#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;
}