6. Curve Motion with Motion Queue

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. MOVS Motion Example

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


// Motion queue MOVS example
void Test_multi_robot_motion_movs(SOCKETFD fd)
{
  queue_motion_set_status(fd, true); // Enable motion queue mode
  sleep(1);
  // Set desired positions
  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 corresponds to how many commands you inserted
  std::cout << "star test moves" << std::endl;
  wait_for_running_over(fd);
  queue_motion_set_status(fd, 0); // Disable the queue
  set_current_mode(fd, 0); // Switch back to teach mode
}


int main() {
  // Connect to the robot
  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); // Wait for connection to succeed
    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;
}