Hi
I want to transfer data in realtime(many different goal position) to my servo for control humanoid robot joint
I use skeleton tracking measure the angle of each joint for control humanoid robot and I have already convert it to position(degree to position) but It cannot write goal position as the data given follow in the picture(1)
My position always equal to zero and not write the next data(it only write zero)
Can you suggest me to solve I have no idea about it
Thank you
Here is my code
I use MX-28 protocol 1 model
sample.getservo1() is my parameter to store int data (0-4095)
but servo cannot write goal position follow my parameter
Please can you solve this problem and I stuck this for a while
I am truely appreciate
int servotest()
{
#define ADDR_MX_TORQUE_ENABLE 24 // Control table address is different in Dynamixel model
#define ADDR_MX_GOAL_POSITION 30
#define ADDR_MX_PRESENT_POSITION 36
// Protocol version
#define PROTOCOL_VERSION 1.0 // See which protocol version is used in the Dynamixel
// Default setting
#define DXL_ID 246 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME “COM3” // Check which port is being used on your controller
// ex) Windows: “COM1” Linux: “/dev/ttyUSB0” Mac: “/dev/tty.usbserial-*”
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE 0
#define DXL_MAXIMUM_POSITION_VALUE 1100 // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
#define DXL_MOVING_STATUS_THRESHOLD 10 // Dynamixel moving status threshold
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
int index;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
int dxl_goal_position = sample.getservo1(); // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl_present_position = 0; // Present position
if (user_number == 0)
{
std::cout << "dont have user " << std::endl;
//return 0;
}
else {
std::cout << "robot and start connect" << std::endl;
}
// Open port
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();
}
// Set port 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();
}
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf("Dynamixel has been successfully connected \n");
}
while (1)
{
if (user_number == 0)
{
std::cout << "dont have user " << std::endl;
//return 0;
}
else {
// Write goal position
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position, &dxl_error);
//std::cout << "Servo data: " << sample.getservo1() << std::endl;
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
do
{
// Read present position
dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, ADDR_MX_PRESENT_POSITION, &dxl_present_position, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
std::cout << "Servo data: " << sample.getservo1() << std::endl;
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL_ID, dxl_goal_position, dxl_present_position);
} while ((abs(dxl_goal_position - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD));
std::cout << "Test loop " << user_number << std::endl;
}
}
// Disable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Close port
portHandler->closePort();
return 1;
}
Sorry I am a little rusty here… But I don’t see enough code here know what is maybe going on. The only place I see dxl_goal_position set is:
int dxl_goal_position = sample.getservo1(); // Goal position
Then you use it to output:
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position, &dxl_error);
And you print it:
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL_ID, dxl_goal_position, dxl_present_position);
So my guess is your one call to sample.getservo1(); is returning 0… But don’t see the sources to this,
now I can fix the data become to zero because when I define int dxl_goal_position = sample.getservo1(); it seem like dxl_goal_position make my data become to zero so I replace dxl_goal_position by sample.getservo1() as an out put so it work you can see in the picture(1),(2) but it seem like packethandler write only the first data but the next data it does not write my question is do you have any idea to make servo write all the data while processing
here is my code
int servotest()
{
#define ADDR_MX_TORQUE_ENABLE 24 // Control table address is different in Dynamixel model
#define ADDR_MX_GOAL_POSITION 30
#define ADDR_MX_PRESENT_POSITION 36
// Protocol version
#define PROTOCOL_VERSION 1.0 // See which protocol version is used in the Dynamixel
// Default setting
#define DXL_ID 246 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME “COM3” // Check which port is being used on your controller
// ex) Windows: “COM1” Linux: “/dev/ttyUSB0” Mac: “/dev/tty.usbserial-*”
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE 0
#define DXL_MAXIMUM_POSITION_VALUE 1100 // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
#define DXL_MOVING_STATUS_THRESHOLD 10 // Dynamixel moving status threshold
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
int index;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
int dxl_goal_position = sample.getservo1(); // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl_present_position = 0; // Present position
if (user_number == 0)
{
std::cout << "dont have user " << std::endl;
//return 0;
}
else {
std::cout << "robot and start connect" << std::endl;
}
// Open port
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();
}
// Set port 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();
}
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf("Dynamixel has been successfully connected \n");
}
while (1)
{
sample.getservo1();
if (user_number == 0)
{
std::cout << "dont have user " << std::endl;
//return 0;
}
else {
std::cout << "Before write: " << dxl_goal_position << "Before skel data:" << sample.getservo1() << std::endl;
// Write goal position
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_GOAL_POSITION, sample.getservo1(), &dxl_error);
std::cout << "after write: " << dxl_goal_position << "after skel data:" << sample.getservo1() << std::endl;
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
do
{
// Read present position
dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, ADDR_MX_PRESENT_POSITION, &dxl_present_position, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL_ID, sample.getservo1(), dxl_present_position);
} while ((abs(sample.getservo1() - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD));
std::cout << "Test loop " << user_number << std::endl;
}
}
// Disable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Close port
portHandler->closePort();
return 1;
}
@THANABOEDEE_WICHAKAN
It makes difficult to read your code if it is not well formatted.
Please use Ctrl + E in the typing area and paste your code inbetween backticks.
One thing I’ve noticed is an incorrect use of function for Torque enable.
You should use write1ByteTxRx()
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf("Dynamixel has been successfully connected \n");
}
int servotest()
{
#define ADDR_MX_TORQUE_ENABLE 24 // Control table address is different in Dynamixel model
#define ADDR_MX_GOAL_POSITION 30
#define ADDR_MX_PRESENT_POSITION 36
// Protocol version
#define PROTOCOL_VERSION 1.0 // See which protocol version is used in the Dynamixel
// Default setting
#define DXL_ID 246 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME "COM3" // Check which port is being used on your controller
// ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE 0
#define DXL_MAXIMUM_POSITION_VALUE 1100 // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
#define DXL_MOVING_STATUS_THRESHOLD 10 // Dynamixel moving status threshold
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
int index;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
int dxl_goal_position = sample.getservo1(); // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl_present_position = 0; // Present position
if (user_number == 0)
{
std::cout << "dont have user " << std::endl;
//return 0;
}
else {
std::cout << "robot and start connect" << std::endl;
}
// Open port
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();
}
// Set port 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();
}
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf("Dynamixel has been successfully connected \n");
}
while (1)
{
sample.getservo1();
if (user_number == 0)
{
std::cout << "dont have user " << std::endl;
//return 0;
}
else {
std::cout << "Before write: " << dxl_goal_position << "Before skel data:" << sample.getservo1() << std::endl;
// Write goal position
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_GOAL_POSITION, sample.getservo1(), &dxl_error);
std::cout << "after write: " << dxl_goal_position << "after skel data:" << sample.getservo1() << std::endl;
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
do
{
// Read present position
dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, ADDR_MX_PRESENT_POSITION, &dxl_present_position, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL_ID, sample.getservo1(), dxl_present_position);
} while ((abs(sample.getservo1() - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD));
std::cout << "Test loop " << user_number << std::endl;
}
}
// Disable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Close port
portHandler->closePort();
return 1;
}
Thank you will
My question is How can I write this code to make servo write all the data while processing if you have already seen my picture it seem like packethandler write the goal position for the first data.
Again I am rusty the Dynamixel SDK and the like and I have no MX servos… Have AX (Protocol 1) and XL (protocol 2) servos. And ahve not been doing much with them for awhile, although about to get back to playing with them.
If it were me, I would step back for a second and see if everything is working.
Things I don’t know from your posting include:
how this is built… Obviously there is additional code not shown. I am assuming windows as it shows COM3… Not sure how PC is talking to servos (U2D2, OpenCM, OpenCR…)… Or how powered…
But one of the first things I would do is to run some example: like for openCM the Dynamixel SDK Protocol 1 sketch read_write_in_loop and see if the servo moves or not…
Can you control the servo using Robotis PC programs?
Sorry not much to go on.
Yes I can
I think the problem is now my program only write the first thread, Normally my program calculate the angle from depth sensor so it has many thread but when use this angle data from depth sensor to control servo it seem like the program can write only the first thread and finish maybe the problem is my thread is not safe I should use something about Mutex to manage my data.
I think opening and closing port every time you update the goal position of DYNAMIXEL isn’t ideal as it may take some time and may not work if the port is held by the system.
I’d recommend to append all the definitions and portHandler, packetHandler object in the global section of your code, and only put the content in the while loop in the servotest().
In this way, you can avoid wasting time and risking unsuccessful serial port activity.
I’m not sure what your code structure looks like, but it could be something like…
#define ADDR_MX_TORQUE_ENABLE 24
#define ADDR_MX_GOAL_POSITION 30
// other definitions and port/packet object initialization followed by
int main()
{
// Your code
// setup a callback function for servotest()
// Open port
portHandler->openPort()
// set baudrate
portHandler->setBaudRate(BAUDRATE)
// enable DYNAMIXEL Torque
packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
}
int servotest()
{
// code in the while loop goes here
}Thank you Will
Now I can solve the data problem but I have some question
-What time of write and read packet take in one cycle?
It varies by many different conditions, but you can esitmate as below.
For example, when requesting to read 1 byte Present Temperature from one MX-28 module, the instruction is 8 bytes and the status packet from the MX-28 is 7 bytes as below.
If DYNAMIXEL uses 57600bps,
57600 bit/s = 57600 / 8 byte/sec = 7200 byte/sec
Read Instruction : (8 byte) / (7200 byte/sec) = 0.001111 sec = 1.11 ms
Status Packet : (7 byte) / (7200 byte/sec) = 0.0009722 sec = 0.97 ms
Considering some delays in processing data and hardware latency across various devices, I’d conservatively consider about 3ms.
All in all, 1.11 + 0.97 + 3 = 5.08 ms
The actual time will be quite different from this estimation, so just take my calculation as a grain of salt.
Thanks.
Thank you Will
Thank for all your information that you give me by your hand 
Note: I don’t know your setup. But for example with Arduino, I would typically do something like:
unit32_t start_time = millis();
Do something like your query
uint32_t delta_time = millis() - start_time;
Which would give delta in milliseconds… Might use micros() instead…
On Linux I have wrappers to do this:
unsigned long millis(void)
{
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts );
return ( ts.tv_sec * 1000 + ts.tv_nsec / 1000000L );
}
unsigned long micros(void)
{
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts );
return ( ts.tv_sec * 1000000L + ts.tv_nsec / 1000L );
}
Alternatively I set an IO pin at the start of the operation and clear it after and measure with logic analyzer
Thank you Kurt
Your information is very useful