纳博特科技开放平台

Appearance

使用 star_servo_point_position_motion_control() 来进行伺服控制运动 ​

伺服控制运动模式是每个通讯周期将点位直接下发给伺服进行运动 ​

cpp
struct ServoPointMovePara
{
	bool end;                       ///< 是否清除之前发送的，未开始插补计算的点位
	int sum;                         ///< 总共要发的帧数
	int count;                           ///< 当前为第几帧
	std::vector<std::vector<double>> pos;   ///< 二维数组，一维表示本次传输的点位数，二维长度为12，各个关节角度或笛卡尔坐标  坐标：机器人（前7个）+ 外部轴（后5个）
};

注意：发送的数据只有当总帧数sum = count的时候机器人才会运行，所以当sum≠1的时候，count就需要从1开始发送，一直累加到sum的值后，机器人才会开始按照对应下发的点位进行运动（具体可以看示例二，示例二就是sum = 10的情况，count = 1开始传到接口servo_point_position_motion_control中，count = 2传给servo_point_position_motion_control， count = 3 ..........一直到count = 10传给接口servo_point_position_motion_control后，下发的全部点位才会开始执行

示例1：封装servo_point_position_motion_control()接口，将目标点位发送给伺服进行运动，每次只发送一个点给伺服 ​

cpp
void test_servo_point_position_motion_control()
{
  
  ServoPointMovePara servoMove;
  servoMove.end = 0;
  servoMove.sum = 1;
  servoMove.count = 1;
  std::vector<double> pos = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};  //前7位为机器人本体，后5位为外部轴
  for (int i = 0; i < 100000; i++)
  {
    servoMove.pos.push_back({pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
    , pos[7], pos[8], pos[9], pos[10], pos[11]});
    std::cout << "servo_point_position_motion_control return  " << servo_point_position_motion_control(fd_7000, servoMove) << std::endl;
    servoMove.pos.clear();
    pos[0] += 0.001;
    pos[8] += 0.001;
    printf("pos : %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f", pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
    , pos[7], pos[8], pos[9], pos[10], pos[11]);
  }
}

示例2：封装servo_point_position_motion_control()接口，将目标点位发送给伺服进行运动，每次最大可发送600个点位 ​

cpp
void test_servo_point_position_motion_control()
{
  
  ServoPointMovePara servoMove;
  servoMove.end = 0;
  servoMove.sum = 10;
  servoMove.count = 1;
  std::vector<double> tar_pos;
  std::vector<double> pos_sync;
  get_current_position(fd, 0, tar_pos);
  get_current_extra_position(fd, pos_sync);
  std::vector<double> pos = {tar_pos[0], tar_pos[1], tar_pos[2], tar_pos[3], tar_pos[4], tar_pos[5], tar_pos[6], 
  pos_sync[0], pos_sync[1], pos_sync[2], pos_sync[3], pos_sync[4]};  //前7位为机器人本体，后5位为外部轴
  for (int i = 0; i < 10; i++)
  {
    for (int j = 0; j < 600; j++)        //存最大600个点位
    {
      servoMove.pos.push_back({pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
      , pos[7], pos[8], pos[9], pos[10], pos[11]});
      if (i < 100) {
        pos[0] += 0.008;
        pos[8] += 0.008;
      } else if ( i >= 200 && i < 300) {
        pos[0] -= 0.008;
        pos[8] -= 0.008;
      } else if ( i >= 300 && i < 400) {
        pos[0] += 0.008;
        pos[8] += 0.008;
      } else {
        pos[0] -= 0.008;
        pos[8] -= 0.008;
      }
    }
    std::cout << "servo_point_position_motion_control return  " << servo_point_position_motion_control(fd_7000, servoMove) << std::endl;
    servoMove.pos.clear();
    if (servoMove.count == 10)
    {
      servoMove.count = 0;
    }
    servoMove.count ++;
  }
}

示例3：封装servo_point_position_motion_control()接口，当出现发送点位不运动，或者缓存区已经满的情况下，发送 ​

cpp
void test_clean_motion()
{
  ServoPointMovePara servoMove;
  servoMove.end = 1;
  servoMove.sum = 1;
  servoMove.count = 1;
  std::vector<double> pos = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};  //前7位为机器人本体，后5位为外部轴
    servoMove.pos.push_back({pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
    , pos[7], pos[8], pos[9], pos[10], pos[11]});
    std::cout << "servo_point_position_motion_control return  " << servo_point_position_motion_control(fd_7000, servoMove) << std::endl;
}

示例：当出现发送点位不运动，或者缓存区已经满的情况下，发送下面的代码 ​

cpp
#include <iostream>
#include <vector>
#include <chrono>
#include "cpp_interface/nrc_interface.h"


int main()
{
  int fd_7000;
  connect(fd_7000, "192.168.1.13");
  enable_servo_position_motion_control(fd_7000, 1);     //开启伺服控制模式
  test_servo_point_position_motion_control();
  //当需要清除点位的时候再调用下面的接口
  // test_clean_motion();
}