Dynamixel MX 64 AR Control with OpenCM Exp+ Open CM 9.04

Support DYNAMIXEL
1

Hello All,

I am using Dynamixel MX Series 64 AR Servos and using Open CM Exp+ Open CM 9.04 hardware. Can anyone provide me a sample code to run the servos? Currently, I am using the following code

/*

  • Dynamixel : MX-series with Protocol 2.0(MX-28 2.0, MX-64 2.0, MX-106 2.0)

  • Controller : OpenCM9.04C + OpenCM 485 EXP

  • Power Source : SMPS 12V 5A

  • MX-Series are connected to Dynamixel BUS on OpenCM 485 EXP board

  • OpenCM 485 EXP

  • This example will test only one Dynamixel at a time.
    */

    #include <DynamixelSDK.h>

    // Control table address (MX-series with Protocol 2.0)
    #define ADDR_MX_TORQUE_ENABLE 64 // Control table address is different in Dynamixel model
    #define ADDR_MX_GOAL_POSITION 116
    #define ADDR_MX_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 BAUDRATE 57600
    #define DEVICENAME “3” //DEVICENAME “1” → Serial1(OpenCM9.04 DXL TTL Ports)
    //DEVICENAME “2” → Serial2
    //DEVICENAME “3” → Serial3(OpenCM 485 EXP)
    #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_MAXIMUM_POSITION_VALUE 4000 // 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 20 // Dynamixel moving status threshold

    #define ESC_ASCII_VALUE 0x1b

    void setup() {
    // put your setup code here, to run once:
    Serial.begin(115200);
    while(!Serial);

    Serial.println(“Start…”);

    // 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

    uint8_t dxl_error = 0; // Dynamixel error
    int32_t dxl_present_position = 0; // Present position

    // Open port
    if (portHandler->openPort())
    {
    Serial.print(“Succeeded to open the port!\n”);
    }
    else
    {
    Serial.print(“Failed to open the port!\n”);
    Serial.print(“Press any key to terminate…\n”);
    return;
    }

    // Set port baudrate
    if (portHandler->setBaudRate(BAUDRATE))
    {
    Serial.print(“Succeeded to change the baudrate!\n”);
    }
    else
    {
    Serial.print(“Failed to change the baudrate!\n”);
    Serial.print(“Press any key to terminate…\n”);
    return;
    }

    // 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)
    {
    packetHandler->getTxRxResult(dxl_comm_result);
    }
    else if (dxl_error != 0)
    {
    packetHandler->getRxPacketError(dxl_error);
    }
    else
    {
    Serial.print(“Dynamixel has been successfully connected \n”);
    }

    while(1)
    {
    Serial.print(“Press any key to continue! (or press q to quit!)\n”);

     while(Serial.available()==0);

 int ch;

 ch = Serial.read();
 if (ch == 'q')
   break;

 // Write goal position
 dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index], &dxl_error);
 if (dxl_comm_result != COMM_SUCCESS)
 {
   packetHandler->getTxRxResult(dxl_comm_result);
 }
 else if (dxl_error != 0)
 {
   packetHandler->getRxPacketError(dxl_error);
 }

 do
 {
   // Read present position
   dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_MX_PRESENT_POSITION, (uint32_t*)&dxl_present_position, &dxl_error);
   if (dxl_comm_result != COMM_SUCCESS)
   {
     packetHandler->getTxRxResult(dxl_comm_result);
   }
   else if (dxl_error != 0)
   {
     packetHandler->getRxPacketError(dxl_error);
   }

   Serial.print("[ID:");      Serial.print(DXL_ID);
   Serial.print(" GoalPos:"); Serial.print(dxl_goal_position[index]);
   Serial.print(" PresPos:");  Serial.print(dxl_present_position);
   Serial.println(" ");


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

 // Change goal position
 if (index == 0)
 {
   index = 1;
 }
 else
 {
   index = 0;
 }

    }

    // 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)
    {
    packetHandler->getTxRxResult(dxl_comm_result);
    }
    else if (dxl_error != 0)
    {
    packetHandler->getRxPacketError(dxl_error);
    }

    // Close port
    portHandler->closePort();

    }

    void loop() {
    // put your main code here, to run repeatedly:

    }

I want to see a sample main code to run servos.

2

@YogangSingh,
Hi, there is a sample code for OpenCM9.04.
If you have installed OpenCM9.04 board manager from Arduino IDE, you’ll be able to find the examples from File > Examples > OpenCM9.04 > 07_DynamixelSDK > protocol2.0 > read_write_mx

Thank you.