...
POST No. 2406687
XM430 R350
2018-02-01 15:48:15 hassity

Yes, I set the buadrate 57600.  Here are some of the control table address and Buadrate. I also attached the new code file.  File name " rw_new.txt"


#defineDXL_ID 1                  

 

#defineDXL2_ID 2

 

#defineDXL3_ID 3

 

#defineDXL4_ID 4

 

#defineBAUDRATE 57600

 

#defineDEVICENAME"COM3"


#defineTORQUE_ENABLE 1

 

#defineTORQUE_DISABLE 0

 

#defineDXL_MINIMUM_POSITION_VALUE 0

 

#defineDXL_2_MINIMUM_POSITION_VALUE 0

 

#defineDXL_3_MINIMUM_POSITION_VALUE 0

 

#defineDXL_4_MINIMUM_POSITION_VALUE 0

 

#defineDXL_MAXIMUM_POSITION_VALUE 4095

 

#defineDXL_2_MAXIMUM_POSITION_VALUE 4095

 

#defineDXL_3_MAXIMUM_POSITION_VALUE 4095

 

#defineDXL_4_MAXIMUM_POSITION_VALUE 4095

 

#defineDXL_MOVING_STATUS_THRESHOLD 20


2018-02-01 15:48:15
hassity
2018-02-05 17:22:51 Will Son

Hi,


Thank you for posting the code.

Although I didn't have chance to run your code, but I took some time to look into your code.

If you are using OpenCM9.04 + 485 EXP board, you need to set the port as below.

#define DEVICENAME "3" // Set this instead of "COM3" 


You need to differentiate dxl_present_position for ID #1 and ID #2 as your code will only print the present position of ID#2 in the do-while loop.

dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_PRESENT_POSITION, (uint32_t*)&dxl_1_present_position, &dxl_error); 

dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_PRESENT_POSITION, (uint32_t*)&dxl_2_present_position, &dxl_error); 

above code is paired with 

printf("[ID:%03d] GoalPos:%03d  PresPos:%03dn", DXL_ID, dxl_goal_position[index], dxl_1_present_position);

printf("[ID:%03d] GoalPos:%03d  PresPos:%03dn", DXL2_ID, dxl_goal_position[index], dxl_2_present_position);


and the while loop condition...

while ((abs(dxl_goal_position[index] - dxl_1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (abs(dxl_goal_position[index] - dxl_2_present_position) > DXL_MOVING_STATUS_THRESHOLD)) 


Also, your code seems like running only once as do-while loop only include reading Dynamixels.

2018-02-05 17:22:51
Will Son
2018-02-05 18:19:42 hassen

Thank you so much for the review and comments. You are helping me a lot. But after applying the comments above the result was as it is shown in the figure. Can you please check it one more time  ? Actually I am using OpenCM 485 EXP with USB2Dyn device.

Thank you.


 
 

2018-02-05 18:19:42
hassen
2018-02-06 17:23:30 Will Son

Did you set both of your XM430's baudrate to 57,600bps? it seems like they are not receiving instruction packets.

The "bulk_read_write" example in the OpenCM9.04 Arduino package (Examples > OpenCM9.04 > Dynamixel SDK > Protocol 2.0 > bulk_read_write) might be helpful to you.

2018-02-06 17:23:30
Will Son
2018-02-07 10:14:34 Will Son

Please use R+ Manager 2.0 to check the XM430 EEPROM settings.

Make sure that their Baudrates are set to "1"(57,600bps) with appropriate IDs.

2018-02-07 10:14:34
Will Son
2018-02-07 16:35:32 hassen


 

2018-02-07 16:35:32
hassen
2018-02-07 16:37:17 hassen

Here is the XM430 EEPROM settings USING R+ manager. The recent file is attached above under the first post. it is named "rw_new.txt"  

2018-02-07 16:37:17
hassen
2018-02-08 14:09:04 Will Son

I just found unusual switch-case statement in your code which may not run properly.


switch (pressed_key) {

  case : key_1;  // Shouldn't this be "case key_1:" ?

    dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index], &dxl_error);

    break;

2018-02-08 14:09:04
Will Son
2018-02-08 17:12:40 hassen

Thank you dear Will Son, 

I've corrected it. But still the result is as it is shown on the figure.  I don't really know understand why thus is happening.  Can you please once go through the whole code ?


Thank you so much


 

2018-02-08 17:12:40
hassen
2018-02-09 10:22:32 Will Son

Have you tried running the read_write example from Dynamixel SDK?

I think it'd be better starting with a simple example and build up from there by adding one Dynamixel at a time.

I usually don't consult with a custom source as it will take too much time to figure out what went wrong, I'd make this case as an exception because it is fairly simple modification from the example code.

Below is the working code that I just tested.

Although all syntax in your code was correct, the reason your code was not working because of the algorithm.

Please use below code only as an example and notice that each Dynamixel is running within a switch-case statement.


// Available Dynamixel model on this example : All models using Protocol 2.0

// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL.

// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below "#define"d variables yourself.

// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M])

//


#ifdef __linux__

#include

#include

#include

#elif defined(_WIN32) || defined(_WIN64)

#include

#endif


#include

#include


#include "dynamixel_sdk.h"                                  // Uses Dynamixel SDK library


// Control table address

#define ADDR_PRO_TORQUE_ENABLE          64                 // Control table address is different in Dynamixel model

#define ADDR_PRO_GOAL_POSITION          116

#define ADDR_PRO_PRESENT_POSITION       132


// Protocol version

#define PROTOCOL_VERSION                2.0                 // See which protocol version is used in the Dynamixel


// Default setting

#define DXL_ID                          1                   // Dynamixel ID: 1

#define DXL2_ID                         2

#define DXL3_ID                         3

#define DXL4_ID                         4

#define BAUDRATE                        1000000

#define DEVICENAME                      "COM3"      // Check which port is being used on your controller

// ex) Windows: "COM1"   Linux: "/dev/ttyUSB0"


#define TORQUE_ENABLE                   1                   // Value for enabling the torque

#define TORQUE_DISABLE                  0                   // Value for disabling the torque

#define DXL_MINIMUM_POSITION_VALUE      0             // Dynamixel will rotate between this value

#define DXL_2_MINIMUM_POSITION_VALUE    0  

#define DXL_3_MINIMUM_POSITION_VALUE    0  

#define DXL_4_MINIMUM_POSITION_VALUE    0  

#define DXL_MAXIMUM_POSITION_VALUE      4095   

#define DXL_2_MAXIMUM_POSITION_VALUE    4095 // 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_3_MAXIMUM_POSITION_VALUE    4095

#define DXL_4_MAXIMUM_POSITION_VALUE    4095

#define DXL_MOVING_STATUS_THRESHOLD     20                  // Dynamixel moving status threshold


#define ESC_ASCII_VALUE                 0x1b

#define key_1                           0x31         // number one  key

#define key_2                           0x32         // number two key 

#define key_3                           0x33         //  number three key

#define key_4                           0x34


int getch()

{

#ifdef __linux__

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;

#elif defined(_WIN32) || defined(_WIN64)

return _getch();

#endif

}


int kbhit(void)

{

#ifdef __linux__

struct termios oldt, newt;

int ch;

int oldf;


tcgetattr(STDIN_FILENO, &oldt);

newt = oldt;

newt.c_lflag &= ~(ICANON | ECHO);

tcsetattr(STDIN_FILENO, TCSANOW, &newt);

oldf = fcntl(STDIN_FILENO, F_GETFL, 0);

fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);


ch = getchar();


tcsetattr(STDIN_FILENO, TCSANOW, &oldt);

fcntl(STDIN_FILENO, F_SETFL, oldf);


if (ch != EOF)

{

ungetc(ch, stdin);

return 1;

}


return 0;

#elif defined(_WIN32) || defined(_WIN64)

return _kbhit();

#endif

}


int main()

{

// Initialize PortHandler instance

// Set the port path

// Get methods and members of PortHandlerLinux or PortHandlerWindows

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 = 0;

int dxl_comm_result = COMM_TX_FAIL;             // Communication result

int dxl_goal_position[2] = { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE };         // Goal position

int dxl_2_goal_position[2] = { DXL_2_MINIMUM_POSITION_VALUE, DXL_2_MAXIMUM_POSITION_VALUE };

int dxl_3_goal_position[2] = { DXL_3_MINIMUM_POSITION_VALUE, DXL_3_MAXIMUM_POSITION_VALUE };

int dxl_4_goal_position[2] = { DXL_4_MINIMUM_POSITION_VALUE, DXL_4_MAXIMUM_POSITION_VALUE };


uint8_t dxl_error = 0;                          // Dynamixel error

int32_t dxl_present_position = 0;               // Present position

int32_t dxl_2_present_position = 0;

int32_t dxl_3_present_position = 0;

int32_t dxl_4_present_position = 0;


// 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();

return 0;

}


// 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();

return 0;

}


// Enable Dynamixel Torque

dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);

if (dxl_comm_result != COMM_SUCCESS)

{

packetHandler->printTxRxResult(dxl_comm_result);

}

else if (dxl_error != 0)

{

packetHandler->printRxPacketError(dxl_error);

}

else

{

printf("Dynamixel #1 has been successfully connected n");

}


// Enable Dynamixe2 Torque

dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);

if (dxl_comm_result != COMM_SUCCESS)

{

packetHandler->printTxRxResult(dxl_comm_result);

}

else if (dxl_error != 0)

{

packetHandler->printRxPacketError(dxl_error);

}

else

{

printf("Dynamixel #2 has been successfully connected n");

}

while (1)

{

// printf("Press key_1  to continue! (or press ESC to quit!)n");

// if (getch() == ESC_ASCII_VALUE)

// break;

// Write goal position

char pressed_key;

printf("Press key to continue! (or press ESC to quit!)n");

pressed_key = getch();

if (pressed_key == ESC_ASCII_VALUE)

break;

switch (pressed_key) {

case key_1:

dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index], &dxl_error);

if (dxl_comm_result != COMM_SUCCESS)

{

packetHandler->printTxRxResult(dxl_comm_result);

}

else if (dxl_error != 0)

{

packetHandler->printRxPacketError(dxl_error);

}

do

{

// Read present position

dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_PRESENT_POSITION, (uint32_t*)&dxl_present_position, &dxl_error);

if (dxl_comm_result != COMM_SUCCESS)

{

packetHandler->printTxRxResult(dxl_comm_result);

}

else if (dxl_error != 0)

{

packetHandler->printRxPacketError(dxl_error);

}

    

printf("[ID:%03d] GoalPos:%03d  PresPos:%03dn", DXL_ID, dxl_goal_position[index], dxl_present_position);

} while (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD);

break;

case key_2:

dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl_2_goal_position[index], &dxl_error);

if (dxl_comm_result != COMM_SUCCESS)

{

packetHandler->printTxRxResult(dxl_comm_result);

}

else if (dxl_error != 0)

{

packetHandler->printRxPacketError(dxl_error);

}

do

{

dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_PRESENT_POSITION, (uint32_t*)&dxl_2_present_position, &dxl_error);

if (dxl_comm_result != COMM_SUCCESS)

{

packetHandler->printTxRxResult(dxl_comm_result);

}

else if (dxl_error != 0)

{

packetHandler->printRxPacketError(dxl_error);

}

printf("[ID:%03d] GoalPos:%03d  PresPos:%03dn", DXL2_ID, dxl_2_goal_position[index], dxl_2_present_position);

} while (abs(dxl_2_goal_position[index] - dxl_2_present_position) > DXL_MOVING_STATUS_THRESHOLD);

break;

default:

break;

}


// Change goal position

if (index == 0)

{

index = 1;

}

else

{

index = 0;

}

}

// Disable Dynamixel Torque

dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);

if (dxl_comm_result != COMM_SUCCESS)

{

packetHandler->printTxRxResult(dxl_comm_result);

}

else if (dxl_error != 0)

{

packetHandler->printRxPacketError(dxl_error);

}


// Close port

portHandler->closePort();


return 0;


}

2018-02-09 10:22:32
Will Son
2018-02-12 10:56:13 hassen

Dear Will Son, 

Thank you so much really ! It works well.

2018-02-12 10:56:13
hassen
Reply
웹에디터 시작 웹 에디터 끝