纳博特科技开放平台

Appearance

6.运动队列的曲线运动 ​

1、封装设置伺服就绪函数 ​

cpp
/*
 * 伺服就绪函数
 */
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、封装等待运动结束的函数 ​

cpp
/*
 * 循环阻塞运动结束函数
 */
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运动示例 ​

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