다이나믹셀 통신 속도가 너무 느려졌습니다!

사용환경
모델: XH540-150W
통신장비: U2D2
os: Ubuntu 20.04
언어: cpp
Baudrate = 1Mps

제가 일전에 아두이노로 다이나믹셀을 제어했을 때 위치제어 통신 속도가 1ms 내외였었는데, 이번에 PC로 환경을 옮기면서 다시 시간을 측정해보니 15ms 내외로 뭔가 문제가 있어보여 문의드립니다. 해당 코드 첨부하여 보내드립니다.

//Include
#include <fcntl.h>
#include <termios.h>
#include <stdlib.h>
#include <stdio.h>
#include
#include <math.h>

#include “dynamixel_sdk.h”

using namespace std;

// Define
#define PI 3.14159265358979323846
#define STDIN_FILENO 0
#define ADDR_OPERATING_MODE 11
#define ADDR_TORQUE_ENABLE 64
#define ADDR_GOAL_POSITION 116
#define ADDR_PRESENT_POSITION 132
#define BAUDRATE 1000000
#define PROTOCOL_VERSION 2.0
#define DXL_ID 0
#define DEVICENAME “/dev/ttyDXL”
#define EXT_POSITION_CONTROL_MODE 4
#define TORQUE_ENABLE 1
#define TORQUE_DISABLE 0
#define DXL_MOVING_STATUS_THRESHOLD 20 // DYNAMIXEL moving status threshold
#define ESC_ASCII_VALUE 0x1b
#define DXL_RESOLUTION 4096
#define INTERVAL_MS 10

constexpr long INTERVAL_MICROS = INTERVAL_MS*1000;

// Global Variables

// Function
int getch() {

struct termios oldt, newt;
int ch;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
}

int main(){

// Open port and packet
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

if (portHandler->openPort()) {
printf(“Succeeded to open the port!\n”);
}
else {
printf(“Failed to open the port!\n”);
printf(“Press any key to terminate…\n”);
getch();
return 0;
}

// Set baudrate
if (portHandler->setBaudRate(BAUDRATE)) {
printf(“Succeeded to change the baudrate!\n”);
}
else {
printf(“Failed to change the baudrate!\n”);
printf(“Press any key to terminate…\n”);
getch();
return 0;
}

// Define status variables
uint8_t dxl_error = 0;
int dxl_comm_result = COMM_TX_FAIL;
int32_t dxl_present_position = 0;

// Set extended pos control mode & zero pos
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
return 0;
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf(“Torque is disabled \n”);
}

dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
return 0;
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf(“Operating mode changed to extended position control mode. \n”);
}

printf(“Set zero pos\n”);
getch();

int32_t pos_zero = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION, (uint32_t*)&dxl_present_position, &dxl_error);

dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
return 0;
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf(“Torque is abled \n”);
}

printf(“Input anything”);
getch();

int32_t pos_trg = pos_zero; // Unit: resolution

chrono::high_resolution_clock::time_point prev = chrono::high_resolution_clock::now();
chrono::high_resolution_clock::time_point curr = chrono::high_resolution_clock::now();

chrono::microseconds term;

bool is_enough = false;
uint32_t idx = 0;

while(true){
//Interval action here
curr = chrono::high_resolution_clock::now();

pos_trg = idx + pos_zero;
packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, pos_trg, &dxl_error);

// if(dxl_comm_result != COMM_SUCCESS){
//   printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
//   printf("something is wrong");
//   break;
// }

idx++;

term = chrono::duration_cast<chrono::microseconds>(curr - prev);
prev = curr;
printf("%ld\n", term.count()); // microsecond

// while(true){
//   curr = chrono::high_resolution_clock::now();
//   term = chrono::duration_cast<chrono::microseconds>(curr - prev);

//   if(term.count() > INTERVAL_MICROS)
//   break;
//   else
//   is_enough = true;
// }
// prev = curr;

// if(!is_enough)
// printf("Not enough!\n");

// is_enough = false;

}

return 0;
}

안녕하세요.

PC에서 느려진거라면 여러가지 원인이 있을 수 있지만 아래 내용을 참고하여 한번 확인 해주세요.

포트 응답 속도 설정

감사합니다.