DynamixelSDK
DynamixelSDK copied to clipboard
ROBOTIS-GIT
Not able to change the speed for Dynamixel MX-106
Hello Everyone,
I am new to dynamixel and programing ROS with c++. I am attempting to lower the speed of Dynamixel motor, I have gone through the eManual and checked the address to change the velocity. I will post my code as well. It is simple read_write_node cpp code with added lines for lower the speed in function setPositionCallback. I tried changing Acceleration, disabling Torque, but nothing seems to work.
`#include <ros/ros.h> #include "std_msgs/String.h" #include "dynamixel_sdk_examples/GetPosition.h" #include "dynamixel_sdk_examples/SetPosition.h" #include "dynamixel_sdk/dynamixel_sdk.h"
using namespace dynamixel;
// Control table address for XL330-M288-T // #define ADDR_TORQUE_ENABLE 64 // #define ADDR_GOAL_POSITION 116 // #define ADDR_PRESENT_POSITION 132
// Control table address for MX-106 #define ADDR_TORQUE_ENABLE 24 #define ADDR_GOAL_POSITION 30 #define ADDR_PRESENT_POSITION 36 #define ADDR_Moving_speed 32 #define ADDR_Present_speed 38 #define ADDR_ACCELERATION 73 #define ADDR_TORQUE_CONTROL_MODE_ENABLE 70
// Protocol version //#define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. #define PROTOCOL_VERSION 1.0 // Default Protocol version of DYNAMIXEL MX series.
// Default setting #define Shoulder_flexion 1 // DXL1 ID #define Shoulder_abduction 2 // DXL2 ID #define Shoulder_rotation 3 // DXL3 ID #define Elbow_flexion 4 // DXL4 ID
#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series #define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command
PortHandler * portHandler; PacketHandler * packetHandler;
bool getPresentPositionCallback( dynamixel_sdk_examples::GetPosition::Request & req, dynamixel_sdk_examples::GetPosition::Response & res) { uint8_t dxl_error = 0; int dxl_comm_result = COMM_TX_FAIL;
// Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(int16_t) for the Position Value. int32_t position = 0; int16_t speed = 0;
// Read Present Position (length : 4 bytes) and Convert uint32 -> int32 // When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead.
dxl_comm_result = packetHandler->read4ByteTxRx( portHandler, (uint8_t)req.id, ADDR_PRESENT_POSITION, (uint32_t *)&position, &dxl_error); if (dxl_comm_result == COMM_SUCCESS) { ROS_INFO("getPosition : [ID:%d] -> [POSITION:%d]", req.id, position); res.position = position; //return true; } else { ROS_INFO("Failed to get position! Result: %d", dxl_comm_result); return false; }
}
void setPositionCallback(const dynamixel_sdk_examples::SetPosition::ConstPtr & msgs) { uint8_t dxl_error = 0; int dxl_comm_result = COMM_TX_FAIL; int dxl_comm_speed_result = COMM_TX_FAIL; int dxl_comm_torque_result = COMM_TX_FAIL;
// Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value. uint32_t position = (unsigned int)msgs->position; // Convert int32 -> uint32 uint16_t moving_speed = (unsigned int)64; // Write Goal Position (length : 4 bytes) // When writing 2 byte data to AX / MX(1.0), use write2ByteTxRx() instead.
dxl_comm_speed_result = packetHandler->write2ByteTxRx( portHandler, (uint8_t)msgs->id, ADDR_Moving_speed, moving_speed, &dxl_error); if (dxl_comm_speed_result == COMM_SUCCESS) { ROS_INFO("setSpeed : [ID:%d] [Speed:%d]", msgs->id, moving_speed); } else { ROS_ERROR("Failed to set speed! Result: %d",&dxl_error); }
dxl_comm_result = packetHandler->write4ByteTxRx( portHandler, (uint8_t)msgs->id, ADDR_GOAL_POSITION, position, &dxl_error); if (dxl_comm_result == COMM_SUCCESS) { ROS_INFO("setPosition : [ID:%d] [POSITION:%d]", msgs->id, msgs->position); } else { ROS_ERROR("Failed to set position! Result: %d", dxl_comm_result); }
}
void DynamixelCoonnectionCheckup(dynamixel::PortHandler *portHandler, dynamixel::PacketHandler *packetHandler) { uint8_t dxl_error = 0; int dxl_shoulder_flexion = COMM_TX_FAIL; int dxl_shoulder_abduction = COMM_TX_FAIL; int dxl_shoulder_rotation = COMM_TX_FAIL; int dxl_Elbow_flexion = COMM_TX_FAIL;
//DynamixelCoonnectionCheckup(); if (!portHandler->openPort()) { ROS_ERROR("Failed to open the port!"); //add reconnection }
if (!portHandler->setBaudRate(BAUDRATE)) { ROS_ERROR("Failed to set the baudrate!"); }
dxl_shoulder_flexion = packetHandler->write1ByteTxRx( portHandler, Shoulder_flexion, ADDR_TORQUE_ENABLE, 1, &dxl_error); if (dxl_shoulder_flexion != COMM_SUCCESS) { ROS_DEBUG("shoulder_flexion: ",Shoulder_flexion); ROS_ERROR("Failed to enable torque for shoulder_flexion, Dynamixel ID %d", Shoulder_flexion); //add reconnection } dxl_shoulder_abduction = packetHandler->write1ByteTxRx( portHandler, Shoulder_abduction, ADDR_TORQUE_ENABLE, 1, &dxl_error); if (dxl_shoulder_abduction != COMM_SUCCESS) { ROS_DEBUG("shoulder_abduction: ",Shoulder_abduction); ROS_ERROR("Failed to enable torque for shoulder_abduction, Dynamixel ID %d", Shoulder_abduction); //add reconnection } dxl_shoulder_rotation = packetHandler->write1ByteTxRx( portHandler, Shoulder_rotation, ADDR_TORQUE_ENABLE, 1, &dxl_error); if (dxl_shoulder_rotation != COMM_SUCCESS) { ROS_DEBUG("shoulder_rotation: ",Shoulder_rotation); ROS_ERROR("Failed to enable torque for shoulder_rotation, Dynamixel ID %d", Shoulder_rotation); //add reconnection } dxl_Elbow_flexion = packetHandler->write1ByteTxRx( portHandler, Elbow_flexion, ADDR_TORQUE_ENABLE, 1, &dxl_error); if (dxl_Elbow_flexion != COMM_SUCCESS) { ROS_DEBUG("Elbow_flexion: ",Elbow_flexion); ROS_ERROR("Failed to enable torque for Elbow_flexion, Dynamixel ID %d", Elbow_flexion); //add reconnection } }
int main(int argc, char ** argv) { portHandler = PortHandler::getPortHandler(DEVICE_NAME); packetHandler = PacketHandler::getPacketHandler(PROTOCOL_VERSION);
DynamixelCoonnectionCheckup(portHandler, packetHandler);
ros::init(argc, argv, "read_write_node"); ros::NodeHandle nh; ros::ServiceServer get_position_srv = nh.advertiseService("/get_position", getPresentPositionCallback); ros::Subscriber set_position_sub = nh.subscribe("/set_position", 10, setPositionCallback); ros::spin();
portHandler->closePort(); return 0; }`
Hello Everyone, Still not able to solve this issue, if anyone have any solution regarding above problem please let me know.
Thank you.
Hello @ameykore, thank you for your time on DYNAMIXEL SDK :)
-
As we have our own issue ticket protocol, you are required to fill the given template when opening ticekt. Please fill out the form.
-
Please remove unneccessary comments for us to give you more advice? this will make your code brief and more readable for us :)
-
Please remove and retest your code with taking out your ROS callback function in order to make sure that it is an issue by your hardware or code.
-
Please attatch a resulting terminal that can explain your issue enoguh.
@ROBOTIS-David Thank you for the reply,
- I am not able to understand what you are taking about ticket protocol or fill the form. Can you send us link for these?
I have made changes mentioned above, but please if you need more changes let me know, I am still learning.
Here is also link from where I got this code. If you navigate through the DynamixelSDK/ros/dynamixel_sdk_examples/src/read_write_node.cpp you will see similar code.
Code with no ROS callbacks
` #include <ros/ros.h> #include "std_msgs/String.h" #include "dynamixel_sdk_examples/GetPosition.h" #include "dynamixel_sdk_examples/SetPosition.h" #include "dynamixel_sdk/dynamixel_sdk.h"
using namespace dynamixel;
// Control table address for MX-106
#define ADDR_TORQUE_ENABLE 24
#define ADDR_GOAL_POSITION 30
#define ADDR_PRESENT_POSITION 36
#define ADDR_Moving_speed 32
#define ADDR_Present_speed 38
#define ADDR_ACCELERATION 73
#define ADDR_TORQUE_CONTROL_MODE_ENABLE 70
// Protocol version
#define PROTOCOL_VERSION 1.0 // Default Protocol version of DYNAMIXEL MX series.
// Default setting
#define Shoulder_flexion 1 // DXL1 ID
#define Shoulder_abduction 2 // DXL2 ID
#define Shoulder_rotation 3 // DXL3 ID
#define Elbow_flexion 4 // DXL4 ID
#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series
#define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command
PortHandler * portHandler;
PacketHandler * packetHandler;
void DynamixelCoonnectionCheckup(dynamixel::PortHandler *portHandler, dynamixel::PacketHandler *packetHandler)
{
uint8_t dxl_error = 0;
int dxl_shoulder_flexion = COMM_TX_FAIL;
int dxl_shoulder_abduction = COMM_TX_FAIL;
int dxl_shoulder_rotation = COMM_TX_FAIL;
int dxl_Elbow_flexion = COMM_TX_FAIL;
//DynamixelCoonnectionCheckup();
if (!portHandler->openPort())
ROS_ERROR("Failed to open the port!");
if (!portHandler->setBaudRate(BAUDRATE))
ROS_ERROR("Failed to set the baudrate!");
dxl_shoulder_flexion = packetHandler->write1ByteTxRx(
portHandler, Shoulder_flexion, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_shoulder_flexion != COMM_SUCCESS)
{
ROS_DEBUG("shoulder_flexion: ",Shoulder_flexion);
ROS_ERROR("Failed to enable torque for shoulder_flexion, Dynamixel ID %d", Shoulder_flexion);
}
dxl_shoulder_abduction = packetHandler->write1ByteTxRx(
portHandler, Shoulder_abduction, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_shoulder_abduction != COMM_SUCCESS)
{
ROS_DEBUG("shoulder_abduction: ",Shoulder_abduction);
ROS_ERROR("Failed to enable torque for shoulder_abduction, Dynamixel ID %d", Shoulder_abduction);
}
dxl_shoulder_rotation = packetHandler->write1ByteTxRx(
portHandler, Shoulder_rotation, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_shoulder_rotation != COMM_SUCCESS)
{
ROS_DEBUG("shoulder_rotation: ",Shoulder_rotation);
ROS_ERROR("Failed to enable torque for shoulder_rotation, Dynamixel ID %d", Shoulder_rotation);
}
dxl_Elbow_flexion = packetHandler->write1ByteTxRx(
portHandler, Elbow_flexion, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_Elbow_flexion != COMM_SUCCESS)
{
ROS_DEBUG("Elbow_flexion: ",Elbow_flexion);
ROS_ERROR("Failed to enable torque for Elbow_flexion, Dynamixel ID %d", Elbow_flexion);
}
}
int main(int argc, char ** argv)
{
portHandler = PortHandler::getPortHandler(DEVICE_NAME);
packetHandler = PacketHandler::getPacketHandler(PROTOCOL_VERSION);
DynamixelCoonnectionCheckup(portHandler, packetHandler);
ros::init(argc, argv, "read_write_node");
ros::NodeHandle nh;
ros::ServiceServer get_position_srv = nh.advertiseService("/get_position", getPresentPositionCallback);
ros::Subscriber set_position_sub = nh.subscribe("/set_position", 10, setPositionCallback);
ros::spin();
portHandler->closePort();
return 0;
}
`
Code with ROS callbacks, in case you need it. ` #include <ros/ros.h> #include "std_msgs/String.h" #include "dynamixel_sdk_examples/GetPosition.h" #include "dynamixel_sdk_examples/SetPosition.h" #include "dynamixel_sdk/dynamixel_sdk.h"
using namespace dynamixel;
// Control table address for MX-106
#define ADDR_TORQUE_ENABLE 24
#define ADDR_GOAL_POSITION 30
#define ADDR_PRESENT_POSITION 36
#define ADDR_Moving_speed 32
#define ADDR_Present_speed 38
#define ADDR_ACCELERATION 73
#define ADDR_TORQUE_CONTROL_MODE_ENABLE 70
// Protocol version
#define PROTOCOL_VERSION 1.0 // Default Protocol version of DYNAMIXEL MX series.
// Default setting
#define Shoulder_flexion 1 // DXL1 ID
#define Shoulder_abduction 2 // DXL2 ID
#define Shoulder_rotation 3 // DXL3 ID
#define Elbow_flexion 4 // DXL4 ID
#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series
#define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command
PortHandler * portHandler;
PacketHandler * packetHandler;
bool getPresentPositionCallback(dynamixel_sdk_examples::GetPosition::Request & req,
dynamixel_sdk_examples::GetPosition::Response & res)
{
uint8_t dxl_error = 0;
int dxl_comm_result = COMM_TX_FAIL;
// Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(int16_t) for the Position Value.
int32_t position = 0;
int16_t speed = 0;
dxl_comm_result = packetHandler->read4ByteTxRx(
portHandler, (uint8_t)req.id, ADDR_PRESENT_POSITION, (uint32_t *)&position, &dxl_error);
if (dxl_comm_result == COMM_SUCCESS)
{
ROS_INFO("getPosition : [ID:%d] -> [POSITION:%d]", req.id, position);
res.position = position;
}
else
{
ROS_INFO("Failed to get position! Result: %d", dxl_comm_result);
return false;
}
}
void setPositionCallback(const dynamixel_sdk_examples::SetPosition::ConstPtr & msgs)
{
uint8_t dxl_error = 0;
int dxl_comm_result = COMM_TX_FAIL;
int dxl_comm_speed_result = COMM_TX_FAIL;
int dxl_comm_torque_result = COMM_TX_FAIL;
// Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value.
uint32_t position = (unsigned int)msgs->position; // Convert int32 -> uint32
uint16_t moving_speed = (unsigned int)64;
dxl_comm_speed_result = packetHandler->write2ByteTxRx(
portHandler, (uint8_t)msgs->id, ADDR_Moving_speed, moving_speed, &dxl_error);
if (dxl_comm_speed_result == COMM_SUCCESS)
ROS_INFO("setSpeed : [ID:%d] [Speed:%d]", msgs->id, moving_speed);
else
ROS_ERROR("Failed to set speed! Result: %d",&dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(
portHandler, (uint8_t)msgs->id, ADDR_GOAL_POSITION, position, &dxl_error);
if (dxl_comm_result == COMM_SUCCESS)
ROS_INFO("setPosition : [ID:%d] [POSITION:%d]", msgs->id, msgs->position);
else
ROS_ERROR("Failed to set position! Result: %d", dxl_comm_result);
}
void DynamixelCoonnectionCheckup(dynamixel::PortHandler *portHandler, dynamixel::PacketHandler *packetHandler)
{
uint8_t dxl_error = 0;
int dxl_shoulder_flexion = COMM_TX_FAIL;
int dxl_shoulder_abduction = COMM_TX_FAIL;
int dxl_shoulder_rotation = COMM_TX_FAIL;
int dxl_Elbow_flexion = COMM_TX_FAIL;
//DynamixelCoonnectionCheckup();
if (!portHandler->openPort())
ROS_ERROR("Failed to open the port!");
if (!portHandler->setBaudRate(BAUDRATE))
ROS_ERROR("Failed to set the baudrate!");
dxl_shoulder_flexion = packetHandler->write1ByteTxRx(
portHandler, Shoulder_flexion, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_shoulder_flexion != COMM_SUCCESS)
{
ROS_DEBUG("shoulder_flexion: ",Shoulder_flexion);
ROS_ERROR("Failed to enable torque for shoulder_flexion, Dynamixel ID %d", Shoulder_flexion);
}
dxl_shoulder_abduction = packetHandler->write1ByteTxRx(
portHandler, Shoulder_abduction, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_shoulder_abduction != COMM_SUCCESS)
{
ROS_DEBUG("shoulder_abduction: ",Shoulder_abduction);
ROS_ERROR("Failed to enable torque for shoulder_abduction, Dynamixel ID %d", Shoulder_abduction);
}
dxl_shoulder_rotation = packetHandler->write1ByteTxRx(
portHandler, Shoulder_rotation, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_shoulder_rotation != COMM_SUCCESS)
{
ROS_DEBUG("shoulder_rotation: ",Shoulder_rotation);
ROS_ERROR("Failed to enable torque for shoulder_rotation, Dynamixel ID %d", Shoulder_rotation);
}
dxl_Elbow_flexion = packetHandler->write1ByteTxRx(
portHandler, Elbow_flexion, ADDR_TORQUE_ENABLE, 1, &dxl_error);
if (dxl_Elbow_flexion != COMM_SUCCESS)
{
ROS_DEBUG("Elbow_flexion: ",Elbow_flexion);
ROS_ERROR("Failed to enable torque for Elbow_flexion, Dynamixel ID %d", Elbow_flexion);
}
}
int main(int argc, char ** argv)
{
portHandler = PortHandler::getPortHandler(DEVICE_NAME);
packetHandler = PacketHandler::getPacketHandler(PROTOCOL_VERSION);
DynamixelCoonnectionCheckup(portHandler, packetHandler);
ros::init(argc, argv, "read_write_node");
ros::NodeHandle nh;
ros::ServiceServer get_position_srv = nh.advertiseService("/get_position", getPresentPositionCallback);
ros::Subscriber set_position_sub = nh.subscribe("/set_position", 10, setPositionCallback);
ros::spin();
portHandler->closePort();
return 0;
}
`
Sorry to come back late, @ameykore
When you click New Issue, a templete will appear.
Hi @ameykore
I'm sorry about the delayed response. Don't worry too much about filling up the form, but could you specify your environment such as:
- OS and version :
- MX-106 firmware (Protocol 1 or 2?) :
- USB-DYNAMIXEL interface (U2D2?) :
In the meantime, I'll review this and get back to you soon. Thanks!
Hey,
Sorry for not replying to your previous email. I just missed the notification.
- We are using Windows 10
- MX-106 is Protocol 2.
- We are not using U2D2 we are connecting USB to ESP and then specific pins from ESP to shield and then shield's 3pin to Dynamixel. We posted a diagram on the forum as well. I haven't checked the forum yet, been busy.
Thank you for your follow-up emails. I am really sorry for not replying to the earlier emails. Let me know if you need other information.
Regards, Amey Kore.
On Wed, Jul 20, 2022, 9:19 PM Will Son @.***> wrote:
Hi @ameykore https://github.com/ameykore
I'm sorry about the delayed response. Don't worry too much about filling up the form, but could you specify your environment such as:
- OS and version :
- MX-106 firmware (Protocol 1 or 2?) :
- USB-DYNAMIXEL interface (U2D2?) :
In the meantime, I'll review this and get back to you soon. Thanks!
— Reply to this email directly, view it on GitHub https://github.com/ROBOTIS-GIT/DynamixelSDK/issues/566#issuecomment-1190929828, or unsubscribe https://github.com/notifications/unsubscribe-auth/AIIYHNUCGEDMZ7OTA4JJ4YTVVCQSLANCNFSM5XBG3KIA . You are receiving this because you were mentioned.Message ID: @.***>