사용환경
모델: 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;
}