ameykore commented 2 years ago

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; }`

ameykore commented 2 years ago

Hello Everyone, Still not able to solve this issue, if anyone have any solution regarding above problem please let me know.

Thank you.

ROBOTIS-David commented 2 years ago

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.

ameykore commented 2 years ago

@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;
}

`

ROBOTIS-David commented 2 years ago

Sorry to come back late, @ameykore

When you click New Issue, a templete will appear.

ROBOTIS-Will commented 2 years ago

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!

ameykore commented 2 years ago

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: @.***>

ROBOTIS-GIT / DynamixelSDK

Not able to change the speed for Dynamixel MX-106 #566

include "std_msgs/String.h"

include "dynamixel_sdk_examples/GetPosition.h"

include "dynamixel_sdk_examples/SetPosition.h"

include "dynamixel_sdk/dynamixel_sdk.h"

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

define PROTOCOL_VERSION 1.0 // Default Protocol version of DYNAMIXEL MX series.

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

include <ros/ros.h>

include <ros/ros.h>