6. 모션 큐의 곡선 운동

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. moves 운동 예시

#include <iostream>
#include <thread>
#include <chrono>
#include "cpp_interface/nrc_interface.h"
#include "cpp_interface/nrc_queue_operate.h"


// 모션 큐에서 movs를 실행하는 예시
void Test_multi_robot_motion_movs(SOCKETFD fd)
{
  queue_motion_set_status(fd, true); // 모션 큐 모드 활성화
  sleep(1);
  // 목표 위치 설정
  std::vector<double> pos{431, 214, 149, 0, 0, 0};
  std::vector<double> pos1{378, 214, 149, 0, 0, 0};
  std::vector<double> pos2{340, 230, 149, 0, 0, 0};
  std::vector<double> pos3{280, 280, 149, 0, 0, 0};
  std::vector<double> pos4{262, 327, 149, 0, 0, 0};
  std::vector<double> pos5{299, 342, 149, 0, 0, 0};
  std::vector<double> pos6{378, 353, 149, 0, 0, 0};
  std::vector<double> pos7{429, 368, 149, 0, 0, 0};
  std::vector<double> pos8{471, 317, 149, 0, 0, 0};
  std::vector<double> pos9{448, 238, 149, 0, 0, 0};
  MoveCmd movecmd;
  movecmd.targetPosType = PosType::data;
  movecmd.targetPosValue = pos;
  movecmd.coord = 1;
  movecmd.velocity = 50;
  movecmd.acc = 50;
  movecmd.dec = 50;
  movecmd.pl = 5;
  std::cout << "queue_motion_push_back_moveJ return" << queue_motion_push_back_moveJ(fd, movecmd) << std::endl;
  movecmd.targetPosValue = pos1;
  movecmd.velocity = 200;
  std::cout << "queue_motion_push_back_moveS return" << queue_motion_push_back_moveS(fd, movecmd) << std::endl;
  movecmd.targetPosValue = pos2;
  std::cout << "queue_motion_push_back_moveS return" << queue_motion_push_back_moveS(fd, movecmd) << std::endl;
  movecmd.targetPosValue = pos3;
  std::cout << "queue_motion_push_back_moveS return" << queue_motion_push_back_moveS(fd, movecmd) << std::endl;
  movecmd.targetPosValue = pos4;
  std::cout << "queue_motion_push_back_moveS return" << queue_motion_push_back_moveS(fd, movecmd) << std::endl;
  movecmd.targetPosValue = pos5;
  std::cout << "queue_motion_push_back_moveS return" << queue_motion_push_back_moveS(fd, movecmd) << std::endl;
  movecmd.targetPosValue = pos6;
  std::cout << "queue_motion_push_back_moveS return" << queue_motion_push_back_moveS(fd, movecmd) << std::endl;
  movecmd.targetPosValue = pos7;
  std::cout << "queue_motion_push_back_moveS return" << queue_motion_push_back_moveS(fd, movecmd) << std::endl;
  movecmd.targetPosValue = pos8;
  std::cout << "queue_motion_push_back_moveS return" << queue_motion_push_back_moveS(fd, movecmd) << std::endl;
  std::cout << "queue_motion_send_to_controller return" << queue_motion_send_to_controller(fd, 9) << std::endl; // 여기에 9는 삽입한 명령 수에 해당
  std::cout << "star test moves" << std::endl;
  wait_for_running_over(fd);
  queue_motion_set_status(fd, 0); // 큐 닫기
  set_current_mode(fd, 0); // 다시 티칭 모드로 전환
}


int main() {
  // 로봇 연결
  std::string robot_ip = "192.168.1.13";
  std::string robot_port = "6001";
  int robot_num = 1;
  fd = connect_robot(robot_ip, robot_port);
  printf("fd = %d\n", fd);
  while (!(get_connection_status(fd) == SUCCESS)) {
    usleep(8000); // 연결 성공 대기
    std::cout << "waiting for connect......" << std::endl;
  }
  std::cout << "connect robot success!" << std::endl;
  clear_error(fd);
  int status;
  get_servo_state(fd, status);
  if(status != 1)
  {
    set_servo_state(fd, 1);
  }
  std::cout<< "star........" << std::endl;
  Test_multi_robot_motion_movs(fd);
  usleep(20000);
  while (true)
  {
    usleep(1000000);
    std::vector<double> pos;
    get_current_position(fd, 1, pos);
    print_vector_double(pos);
    int configuration;
    std::cout << "get_robot_configuration return is "<< 
    get_robot_configuration(fd, configuration) << std::endl;
    std::cout << "configuration is " << configuration << std::endl;
  }
  std::cout << "ending..........."  << std::endl;
  return 0;
}